├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── blocks2.yaml ├── clins_data.yaml ├── elfant.yaml ├── fuPo_handheld.yaml ├── helmet.yaml ├── hilti2022.yaml ├── hilti2023_hhs.yaml ├── hilti2023_robot.yaml ├── kthforest.yaml ├── liosam_data.yaml ├── m2dgr.yaml ├── mcdviral_atv.yaml ├── mcdviral_hhs.yaml ├── mcdviral_livox_atv.yaml ├── mcdviral_livox_hhs.yaml ├── mulran.yaml ├── ntuviral.yaml ├── oxford.yaml ├── stillwh.yaml ├── urbanloco_hk.yaml └── whu.yaml ├── docker ├── Dockerfile ├── Makefile ├── run_fuPo_handheld.sh ├── run_mcdviral.sh ├── run_ntuviral.sh └── run_oxford.sh ├── docs ├── Course.png ├── GM_20230126_SLICT.pdf ├── ba-dum-tsss.gif ├── example.png ├── slam_backbone.png └── thumbnail.png ├── include ├── PoseLocalParameterization.h ├── PreintBase.h ├── STDesc.h ├── basalt │ ├── spline │ │ ├── calib_bias.hpp │ │ ├── ceres_local_param.hpp │ │ ├── ceres_spline_helper.h │ │ ├── ceres_spline_helper_jet.h │ │ ├── posesplinex.h │ │ ├── rd_spline.h │ │ ├── se3_spline.h │ │ ├── so3_spline.h │ │ ├── spline_common.h │ │ └── spline_segment.h │ └── utils │ │ ├── assert.h │ │ ├── eigen_utils.hpp │ │ └── sophus_utils.hpp ├── factor │ ├── GyroAcceBiasAnalyticFactor.h │ ├── GyroAcceBiasFactorTMN.hpp │ ├── Point2PlaneFactorTMN.hpp │ ├── PointToPlaneAnalyticFactor.hpp │ ├── PointToPlaneDisFactor.h │ ├── PoseAnalyticFactor.h │ ├── PreintFactor.h │ ├── RelOdomFactor.h │ └── VelocityAnalyticFactor.h ├── ikdTree │ ├── README.md │ ├── ikd_Tree.cpp │ └── ikd_Tree.h └── utility.h ├── launch ├── blocks2.rviz ├── clins_data.rviz ├── elfant.rviz ├── fuPo_handheld.rviz ├── helmet.rviz ├── hilti2022.rviz ├── hilti2023.rviz ├── kthforest.rviz ├── liom_data.rviz ├── liosam_data.rviz ├── m2dgr.rviz ├── m300.rviz ├── mcdviral.rviz ├── mulran.rviz ├── ntuviral.rviz ├── oxford.rviz ├── oxford2.rviz ├── run_blocks2.launch ├── run_clins_data.launch ├── run_elfant.launch ├── run_elfant_demo.launch ├── run_fuPo_handheld.launch ├── run_helmet.launch ├── run_hilti2022.launch ├── run_hilti2023.launch ├── run_jacobian_test.launch ├── run_kthforest.launch ├── run_liosam_data.launch ├── run_m2dgr.launch ├── run_mcdviral.launch ├── run_mcdviral_slamprior.launch ├── run_mulran.launch ├── run_ntuviral.launch ├── run_oxford.launch ├── run_stillwh.launch ├── run_urbanloco.launch ├── run_whu.launch ├── stillwh.rviz ├── urbanloco.rviz ├── whu.rviz └── wkw.rviz ├── msg ├── FeatureCloud.msg ├── OptStat.msg └── TimeLog.msg ├── package.xml ├── scripts ├── run_helmet.sh ├── run_mcdviral.sh ├── run_mcdviral_slamprior.sh ├── run_ntuviral.sh ├── run_one_bag.sh └── run_oxford.sh ├── src ├── CloudMatcher.hpp ├── Estimator.cpp ├── ImuOdom.cpp ├── MapManager.hpp ├── MergeLidar.cpp ├── PointToMapAssoc.cpp ├── PointToMapAssoc.h ├── Relocalization.cpp ├── STDesc.cpp ├── SensorSync.cpp ├── lidar_converters │ ├── BPearlToOuster.cpp │ ├── HesaiToOuster.cpp │ ├── Livox2ToOuster.cpp │ ├── LivoxToOuster.cpp │ ├── M2DGRToOuster.cpp │ ├── MulranToOuster.cpp │ ├── OusterToVelodyne.cpp │ └── VelodyneToOuster.cpp ├── tmnSolver.cpp └── tmnSolver.h └── srv └── globalMapsPublish.srv /config/clins_data.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 0 3 | 4 | # Transform of coordinates in lidar to body 5 | # The (4,4) entry indicates whether stamp is at beginning (1) or end (0) of scan 6 | lidar_topic: [ 7 | "/os_cloud_node/points" 8 | ] 9 | 10 | merged_lidar_topic: "/merged_pc" 11 | 12 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 13 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 14 | # whether stamp is at beginning (1.000) or end (0.000) of scan 15 | lidar_extr: [ 16 | # Velodyne 17 | -0.01201, -0.99993, -0.00220, -0.098, 18 | 0.99991, -0.01202, 0.00553, 0.003, 19 | -0.00556, -0.00213, 0.99998, 0.132, 20 | 0.0, 0.0, 0.0, 1.0 21 | ] 22 | 23 | # Minimum lidar range to admit 24 | min_range: 0.5 25 | 26 | # IMU topic 27 | imu_topic: /imu1/data 28 | 29 | # Gravity constant 30 | GRAV: 9.82 31 | 32 | # Measurement noises 33 | GYR_N: 5.0e-2 34 | GYR_W: 3.0e-3 35 | ACC_N: 6.0e-1 36 | ACC_W: 8.0e-2 37 | 38 | # Weightage of a basic lidar factor 39 | lidar_weight: 1.0 40 | 41 | # Number of packets to process at time 42 | WINDOW_SIZE: 4 43 | # Number of substates to give to each packet 44 | N_SUB_SEG: 8 45 | 46 | # Downsampling grid size, chose -1 for no downsampling 47 | leaf_size: 0.1 48 | assoc_spacing: 0.4 49 | surfel_map_depth: 16 50 | surfel_min_point: 5 51 | surfel_min_depth: 1 52 | surfel_query_depth: 6 53 | surfel_intsect_rad: 0.1 54 | surfel_min_plnrty: 0.5 55 | 56 | # Distance to surfel and min score thresholds 57 | dis_to_surfel_max: 0.3 58 | score_min: 0.0 59 | 60 | # Downsample rate 61 | ds_rate: [ 62 | 2 63 | ] 64 | 65 | # Threshold to add new keyframe 66 | kf_min_dis: 0.5 67 | kf_min_angle: 10 68 | 69 | # Optimization parameters 70 | lidar_loss_thres: 10.0 71 | 72 | # Solver: 73 | # Trust region strategy: "lm", "dogleg" 74 | trustRegType: "lm" 75 | # Linear Algebra library used: "eigen", "lapack", "cuda" 76 | linAlgbLib: "cuda" 77 | max_solve_time: 0.3 78 | max_iterations: 30 79 | 80 | # Sensors fused 81 | fuse_lidar: 1 82 | fuse_imu: 1 83 | fuse_poseprop: 0 84 | 85 | regularize_imu: 1 86 | imu_init_time: 0.5 87 | max_outer_iters: 3 88 | max_lidar_factor: 4000 # Maximum number of lidar factors 89 | 90 | # Loop closure parameters 91 | loop_en: 1 92 | loop_kf_nbr: 50 # Number of neighbour to check for loop closure 93 | loop_time_mindiff: 10 # Only check for loop when keyframes have this much difference 94 | 95 | icpMaxIter: 100 # Maximum iterations for ICP 96 | icpFitnessThres: 0.3 # Fitness threshold for ICP check 97 | histDis: 15 # Maximum correspondence distance for icp 98 | 99 | # Bundle adjustment params 100 | rib_edge: 5 101 | odom_q_noise: 0.1 102 | odom_p_noise: 0.1 103 | loop_weight: 0.1 104 | 105 | # Number of optimizations before quitting 106 | debug_exit: -1 107 | publish_map: 0 108 | 109 | # Log directory 110 | log_dir: "/home/tmn/slict_log/clins_data" -------------------------------------------------------------------------------- /config/elfant.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/os_cloud_node/points" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Hesai 16 | 0.0, -1.0, 0.0, 0.082, 17 | -1.0, 0.0, 0.0, 0.052, 18 | 0.0, 0.0, -1.0, -0.026, 19 | 0.0, 0.0, 0.0, 1.000 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 1.0 24 | 25 | # IMU topic 26 | imu_topic: "/vn100/imu" 27 | 28 | # Imu data will be multiplied with this to get m/s^2 29 | acce_scale: 1.00 30 | 31 | # Gravity constant 32 | GRAV: 9.82 33 | 34 | # IMU noises 35 | GYR_N: 20 36 | GYR_W: 5 37 | ACC_N: 1 38 | ACC_W: 5 39 | 40 | # Position prior weight 41 | POSE_N: 10 42 | 43 | # Noise of velocity propagation 44 | VEL_N: 10 45 | 46 | # Weightage of a basic lidar factor 47 | lidar_weight: 20 48 | 49 | # Bias bounds 50 | BG_BOUND: [ 0.2, 0.2, 0.3] 51 | BA_BOUND: [ 0.2, 0.2, 0.4] 52 | 53 | # Number of packets to process at time 54 | WINDOW_SIZE: 3 55 | # Number of substates to give to each packet 56 | N_SUB_SEG: 16 57 | # Spline order 58 | SPLINE_N: 4 59 | # Knot length 60 | deltaT: 0.1 61 | # Period at the beginning of the spline to fix 62 | start_fix_span: -0.0 63 | # Period at the end of the spline to fix 64 | final_fix_span: -1.0 65 | # Steps to reassociate 66 | reassociate_steps: 1 67 | # Deskew choice, 0 for IMU prop, 1 for SPLINE, 1 for before and 1 for after 68 | deskew_method: [ 0, 0 ] 69 | # Rate of doing reassociation 70 | reassoc_rate: 1 71 | 72 | # Downsampling grid size, chose -1 for no downsampling 73 | leaf_size: 0.1 # Downsample scale of input pointcloud, also the scale of ufomap leaf 74 | assoc_spacing: 0.8 75 | surfel_map_depth: 16 76 | surfel_min_point: 8 77 | surfel_min_depth: 1 78 | surfel_query_depth: 4 # Equal to Dmax + 1. Dmax is defined in the paper 79 | surfel_intsect_rad: 0.2 # r in the the paper 80 | surfel_min_plnrty: 0.6 81 | 82 | # Distance to surfel and min score thresholds 83 | dis_to_surfel_max: 0.3 84 | score_min: 0.0 85 | 86 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 87 | ds_rate: [ 88 | 1 89 | ] 90 | 91 | # Number of pointclouds to merge into one 92 | sweep_len: 2 93 | 94 | # Threshold to add new keyframe 95 | kf_min_dis: 2.0 96 | kf_min_angle: 20 97 | refine_kf: 0 98 | 99 | # Optimization parameters 100 | lidar_loss_thres: 10.0 101 | imu_loss_thres: -1.0 102 | 103 | # Solver: 104 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 105 | linSolver: "snc" 106 | # Trust region strategy: "lm", "dogleg" 107 | trustRegType: "lm" 108 | # Linear Algebra library used: "eigen", "lapack", "cuda" 109 | linAlgbLib: "eigen" 110 | max_solve_time: 0.3 111 | max_iterations: 30 112 | ensure_real_time: 1 113 | find_factor_cost: 0 114 | fit_spline: 0 115 | 116 | # Sensors fused 117 | fuse_lidar: 1 118 | fuse_imu: 1 119 | fuse_poseprop: 1 120 | fuse_velprop: 0 121 | 122 | regularize_imu: 1 123 | imu_init_time: 0.5 124 | max_outer_iters: 1 125 | max_lidar_factor: 4000 # Maximum number of lidar factors 126 | 127 | # Loop closure parameters 128 | loop_en: 1 129 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 130 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 131 | 132 | icpMaxIter: 100 # Maximum iterations for ICP 133 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 134 | histDis: 15 # Maximum correspondence distance for icp 135 | 136 | # Bundle adjustment params 137 | rib_edge: 100 138 | odom_q_noise: 0.1 139 | odom_p_noise: 0.1 140 | loop_weight: 0.05 141 | 142 | # Number of optimizations before quitting 143 | publish_map: 0 144 | 145 | # Log directory 146 | log_dir: "/home/tmn/slict_log/atv" 147 | 148 | # Configurations for relocalization 149 | relocalization: 150 | 151 | # pre process 152 | ds_size: 0.25 153 | maximum_corner_num: 100 154 | 155 | # key points 156 | plane_detection_thre: 0.01 157 | plane_merge_normal_thre: 0.2 158 | voxel_size: 2.0 159 | voxel_init_num: 10 160 | proj_image_resolution: 0.5 161 | proj_dis_min: 0 162 | proj_dis_max: 5 163 | corner_thre: 10 164 | 165 | # std descriptor 166 | descriptor_near_num: 10 167 | descriptor_min_len: 2 168 | descriptor_max_len: 50 169 | non_max_suppression_radius: 2 170 | std_side_resolution: 0.2 171 | 172 | # candidate search 173 | skip_near_num: 100 174 | candidate_num: 50 175 | sub_frame_num: 1 176 | vertex_diff_threshold: 0.5 177 | rough_dis_threshold: 0.05 178 | normal_threshold: 0.2 179 | dis_threshold: 0.5 180 | icp_threshold: 0.4 181 | 182 | # Number of keyframe to merge for each reloc check 183 | kf_merged: 1 184 | # For guestimation filtering 185 | refine_reloc_tf: 1 186 | reloc_thres: 2 187 | reloc_timespan: -1.0 188 | pose_percentile_thres: 0.7 189 | priormap_viz_res: 0.2 190 | marginalize_new_points: 0 191 | ioa_max_iter: 20 -------------------------------------------------------------------------------- /config/fuPo_handheld.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 0 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | /os_cloud_node/points 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | 1.0, 0.0, 0.0, 0.006253, 16 | 0.0, 1.0, 0.0, -0.011775, 17 | 0.0, 0.0, 1.0, 0.007645, 18 | 0.0, 0.0, 0.0, 1.0 19 | ] 20 | 21 | # Minimum lidar range to admit 22 | min_range: 0.0 23 | 24 | # IMU topic 25 | imu_topic: "/os_cloud_node/imu/data_raw" 26 | 27 | # Gravity constant 28 | GRAV: 9.81007 29 | 30 | # IMU noises 31 | GYR_N: 0.00035986799076537137 32 | GYR_W: 5.511335586835809e-06 33 | ACC_N: 0.007575311319546376 34 | ACC_W: 0.0003157762686059487 35 | 36 | # Weightage of a basic lidar factor 37 | lidar_weight: 1.0 38 | 39 | # Number of packets to process at time 40 | WINDOW_SIZE: 4 41 | # Number of substates to give to each packet 42 | N_SUB_SEG: 2 43 | 44 | # Downsampling grid size, chose -1 for no downsampling 45 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leafs 46 | assoc_spacing: 0.2 47 | surfel_map_depth: 16 48 | surfel_min_point: 5 49 | surfel_min_depth: 1 50 | surfel_query_depth: 7 # Equal to Dmax + 1. Dmax is defined in the paper 51 | surfel_intsect_rad: 0.2 # r in the the paper 52 | surfel_min_plnrty: 0.7 53 | 54 | # Distance to surfel and min score thresholds 55 | dis_to_surfel_max: 0.3 56 | score_min: 0.0 57 | 58 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 59 | ds_rate: [ 60 | 2 61 | ] 62 | 63 | # Threshold to add new keyframe 64 | kf_min_dis: 0.5 65 | kf_min_angle: 10 66 | 67 | # Optimization parameters 68 | lidar_loss_thres: 10.0 69 | 70 | # Solver: 71 | # Trust region strategy: "lm", "dogleg" 72 | trustRegType: "lm" 73 | # Linear Algebra library used: "eigen", "lapack", "cuda" 74 | linAlgbLib: "cuda" 75 | max_solve_time: 0.3 76 | max_iterations: 30 77 | 78 | # Sensors fused 79 | fuse_lidar: 1 80 | fuse_imu: 1 81 | fuse_poseprop: 0 82 | 83 | regularize_imu: 1 84 | imu_init_time: 0.5 85 | max_outer_iters: 3 86 | max_lidar_factor: 4000 # Maximum number of lidar factors 87 | 88 | # Loop closure parameters 89 | loop_en: 1 # Enable loop closure 90 | loop_kf_nbr: 10 # Number of neighbour to check for loop closure 91 | loop_time_mindiff: 10 # Only check for loop when keyframes have this much difference 92 | 93 | icpMaxIter: 100 # Maximum iterations for ICP 94 | icpFitnessThres: 0.3 # Fitness threshold for ICP check 95 | histDis: 15 # Maximum correspondence distance for icp 96 | 97 | # Bundle adjustment params 98 | rib_edge: 5 99 | odom_q_noise: 0.1 100 | odom_p_noise: 0.1 101 | loop_weight: 0.1 102 | 103 | # Number of optimizations before quitting 104 | debug_exit: -1 105 | publish_map: 0 106 | 107 | # Log directory 108 | log_dir: "/home/tmn/slict_log/fuPo_handheld" -------------------------------------------------------------------------------- /config/hilti2022.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 0 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | /os_cloud_node/points 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Horizontal 16 | 0.0, -1.0, 0.0, -0.00100, 17 | -1.0, 0.0, 0.0, -0.00855, 18 | 0.0, 0.0, -1.0, 0.05500, 19 | 0.0, 0.0, 0.0, 1.00000 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 0.5 24 | 25 | # IMU topic 26 | imu_topic: "/alphasense/imu" 27 | 28 | # Gravity constant 29 | GRAV: 9.81007 30 | 31 | # IMU noises 32 | GYR_N: 1.0e-0 33 | GYR_W: 2.66e-4 34 | ACC_N: 5.0e-0 35 | ACC_W: 4.3e-3 36 | 37 | # Weightage of a basic lidar factor 38 | lidar_weight: 1.0 39 | 40 | # Number of packets to process at time 41 | WINDOW_SIZE: 2 42 | # Number of substates to give to each packet 43 | N_SUB_SEG: 2 44 | 45 | # Downsampling grid size, chose -1 for no downsampling 46 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leafs 47 | assoc_spacing: 0.05 48 | surfel_map_depth: 16 49 | surfel_min_point: 10 50 | surfel_min_depth: 1 51 | surfel_query_depth: 6 # Equal to Dmax + 1. Dmax is defined in the paper 52 | surfel_intsect_rad: 0.1 # r in the the paper 53 | surfel_min_plnrty: 0.2 54 | 55 | # Distance to surfel and min score thresholds 56 | dis_to_surfel_max: 0.3 57 | score_min: 0.0 58 | 59 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 60 | ds_rate: [ 61 | 1 62 | ] 63 | 64 | # Threshold to add new keyframe 65 | kf_min_dis: 0.5 66 | kf_min_angle: 10 67 | 68 | # Optimization parameters 69 | lidar_loss_thres: 10.0 70 | 71 | # Solver: 72 | # Trust region strategy: "lm", "dogleg" 73 | trustRegType: "lm" 74 | # Linear Algebra library used: "eigen", "lapack", "cuda" 75 | linAlgbLib: "cuda" 76 | max_solve_time: 0.3 77 | max_iterations: 30 78 | 79 | # Sensors fused 80 | fuse_lidar: 1 81 | fuse_imu: 0 82 | 83 | regularize_imu: 1 84 | imu_init_time: 0.5 85 | max_outer_iters: 3 86 | max_lidar_factor: -1 # Maximum number of lidar factors 87 | 88 | # Loop closure parameters 89 | loop_en: 1 # Enable loop closure 90 | loop_kf_nbr: 10 # Number of neighbour to check for loop closure 91 | loop_time_mindiff: 10 # Only check for loop when keyframes have this much difference 92 | 93 | icpMaxIter: 100 # Maximum iterations for ICP 94 | icpFitnessThres: 0.3 # Fitness threshold for ICP check 95 | histDis: 15 # Maximum correspondence distance for icp 96 | ioa_max_iter: 10 # Maximum iterations of the ioa method 97 | ioa_DJ_end: 0.001 # Terminate ioa when DJ is below this 98 | 99 | # Bundle adjustment params 100 | rib_edge: 5 101 | odom_q_noise: 0.1 102 | odom_p_noise: 0.1 103 | loop_weight: 0.1 104 | 105 | # Number of optimizations before quitting 106 | debug_exit: -1 107 | publish_map: 0 108 | 109 | # Log directory 110 | log_dir: "/home/tmn/slict_log/oxford" -------------------------------------------------------------------------------- /config/kthforest.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/os_cloud_node/points", 7 | "/livox/lidar_ouster" 8 | ] 9 | 10 | merged_lidar_topic: "/merged_pc" 11 | 12 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 13 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 14 | # whether stamp is at beginning (1.000) or end (0.000) of scan 15 | lidar_extr: [ 16 | # Ouster 17 | 0.999913576287622, -0.009328347198916, -0.009264504346060, -0.046532554232541, 18 | -0.009582920539413, -0.999566866147690, -0.027825260210211, -0.031341381304893, 19 | -0.009000929997568, 0.027911645487217, -0.999569874123872, -0.016099826823336, 20 | 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 21 | # Livox 22 | 0.999695761128444, -0.009475673481037, -0.022772921210179, 0.001588648264021, 23 | -0.009916714240125, -0.999763923298728, -0.019332713755702, -0.004894736723337, 24 | -0.022584356700956, 0.019552673562790, -0.999553725289236, 0.086512606238327, 25 | 0, 0, 0, 1.000000000000000 26 | ] 27 | 28 | # Minimum lidar range to admit 29 | min_range: 0.5 30 | 31 | # IMU topic 32 | imu_topic: "/vectornav/IMU" 33 | 34 | # Gravity constant 35 | GRAV: 9.82 36 | 37 | # IMU noises 38 | GYR_N: 5.0e-3 39 | GYR_W: 3.0e-6 40 | ACC_N: 6.0e-2 41 | ACC_W: 8.0e-5 42 | 43 | # Weightage of a basic lidar factor 44 | lidar_weight: 1.0 45 | 46 | # Number of packets to process at time 47 | WINDOW_SIZE: 4 48 | # Number of substates to give to each packet 49 | N_SUB_SEG: 4 50 | 51 | # Downsampling grid size, chose -1 for no downsampling 52 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf 53 | assoc_spacing: 0.8 54 | surfel_map_depth: 16 55 | surfel_min_point: 5 56 | surfel_min_depth: 1 57 | surfel_query_depth: 4 # Equal to Dmax + 1. Dmax is defined in the paper 58 | surfel_intsect_rad: 0.1 # r in the the paper 59 | surfel_min_plnrty: 0.4 60 | 61 | # Distance to surfel and min score thresholds 62 | dis_to_surfel_max: 0.5 63 | score_min: 0.05 64 | 65 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 66 | ds_rate: [ 67 | 2, 68 | 2 69 | ] 70 | 71 | # Threshold to add new keyframe 72 | kf_min_dis: 0.5 73 | kf_min_angle: 10 74 | 75 | # Optimization parameters 76 | lidar_loss_thres: 10.0 77 | 78 | # Solver: 79 | # Trust region strategy: "lm", "dogleg" 80 | trustRegType: "lm" 81 | # Linear Algebra library used: "eigen", "lapack", "cuda" 82 | linAlgbLib: "cuda" 83 | max_solve_time: 0.3 84 | max_iterations: 30 85 | 86 | # Sensors fused 87 | fuse_lidar: 1 88 | fuse_imu: 1 89 | fuse_poseprop: 0 90 | 91 | regularize_imu: 1 92 | imu_init_time: 1.0 93 | max_outer_iters: 3 94 | max_lidar_factor: 4000 # Maximum number of lidar factors 95 | 96 | # Loop closure parameters 97 | loop_en: 1 98 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 99 | loop_kfid_mindiff: 40 # Only check for loop when keyframes have this much difference 100 | 101 | icpMaxIter: 100 # Maximum iterations for ICP 102 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 103 | histDis: 15 # Maximum correspondence distance for icp 104 | 105 | # Bundle adjustment params 106 | rib_edge: 100 107 | odom_q_noise: 0.1 108 | odom_p_noise: 0.1 109 | loop_weight: 0.05 110 | 111 | # Number of optimizations before quitting 112 | debug_exit: -1 113 | publish_map: 0 114 | 115 | # Log directory 116 | log_dir: "/home/tmn/slict_log/kthforest" -------------------------------------------------------------------------------- /config/liosam_data.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 0 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/os_cloud_node/points" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Velodyne 16 | 1, 0, 0, 0, 17 | 0, 1, 0, 0, 18 | 0, 0, 1, 0, 19 | 0, 0, 0, 1 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 0.5 24 | 25 | # IMU topic 26 | imu_topic: "/imu_correct" 27 | 28 | # Gravity constant 29 | GRAV: 9.81007 30 | 31 | # IMU noises 32 | GYR_N: [ 1.5e-0 ] 33 | GYR_W: [ 3.5e-6 ] 34 | ACC_N: [ 4.0e-0 ] 35 | ACC_W: [ 6.4e-5 ] 36 | 37 | # Weightage of a basic lidar factor 38 | lidar_weight: 1.0 39 | 40 | # Number of packets to process at time 41 | WINDOW_SIZE: 4 42 | # Number of substates to give to each packet 43 | N_SUB_SEG: 8 44 | 45 | # Downsampling grid size, chose -1 for no downsampling 46 | leaf_size: 0.05 47 | assoc_spacing: 0.2 48 | surfel_map_depth: 16 49 | surfel_min_point: 8 50 | surfel_min_depth: 1 51 | surfel_query_depth: 6 52 | surfel_intsect_rad: 0.2 53 | surfel_min_plnrty: 0.6 54 | 55 | # Distance to surfel and min score thresholds 56 | dis_to_surfel_max: 0.5 57 | score_min: 0.0 58 | 59 | # Downsample rate 60 | ds_rate: [ 61 | 4 62 | ] 63 | 64 | # Keyframe evaluation 65 | kf_min_dis: 0.5 66 | kf_min_angle: 10 67 | 68 | # Optimization parameters 69 | lidar_loss_thres: 10.0 70 | 71 | # Solver: 72 | # Trust region strategy: "lm", "dogleg" 73 | trustRegType: "lm" 74 | # Linear Algebra library used: "eigen", "lapack", "cuda" 75 | linAlgbLib: "cuda" 76 | max_solve_time: 0.3 77 | max_iterations: 30 78 | 79 | # Sensors fused 80 | fuse_lidar: 1 81 | fuse_imu: 1 82 | fuse_poseprop: 0 83 | 84 | regularize_imu: 1 85 | imu_init_time: 0.1 86 | max_outer_iters: 3 87 | max_lidar_factor: 4000 # Maximum number of lidar factors 88 | 89 | # Loop closure parameters 90 | loop_en: 1 # Enable loop closure 91 | loop_kf_nbr: 10 # Number of neighbour to check for loop closure 92 | loop_time_mindiff: 10 # Only check for loop when keyframes have this much difference 93 | 94 | icpMaxIter: 100 # Maximum iterations for ICP 95 | icpFitnessThres: 0.3 # Fitness threshold for ICP check 96 | histDis: 15 # Maximum correspondence distance for icp 97 | 98 | # Bundle adjustment params 99 | rib_edge: 5 100 | odom_q_noise: 0.1 101 | odom_p_noise: 0.1 102 | loop_weight: 0.1 103 | 104 | # Number of optimizations before quitting 105 | debug_exit: -1 106 | publish_map: 0 107 | 108 | # Log directory 109 | log_dir: "/home/tmn/slict_log/ntu_viral" -------------------------------------------------------------------------------- /config/m2dgr.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/os_cloud_node/points" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | 1.0, 0.0, 0.0, 0.27255, 16 | 0.0, 1.0, 0.0, -0.00053, 17 | 0.0, 0.0, 1.0, 0.17954, 18 | 0.0, 0.0, -0.1, 1.000 19 | ] 20 | 21 | # Minimum lidar range to admit 22 | min_range: 1.0 23 | 24 | # IMU topic 25 | imu_topic: "/handsfree/imu" 26 | 27 | # Gravity constant 28 | GRAV: 9.82 29 | 30 | # IMU noises 31 | GYR_N: 2 32 | GYR_W: 4 33 | ACC_N: 0.2 34 | ACC_W: 4 35 | 36 | # Position prior weight 37 | POSE_N: 20 38 | 39 | # Noise of velocity propagation 40 | VEL_N: 10 41 | 42 | # Weightage of a basic lidar factor 43 | lidar_weight: 2 44 | 45 | # Bias bounds 46 | BG_BOUND: [ 0.1, 0.1, 0.2] 47 | BA_BOUND: [ 0.3, 0.3, 0.4] 48 | 49 | # Number of packets to process at time 50 | WINDOW_SIZE: 3 51 | # Number of substates to give to each packet 52 | N_SUB_SEG: 16 53 | # Spline order 54 | SPLINE_N: 4 55 | # Knot length 56 | deltaT: 0.01 57 | # Period at the beginning of the spline to fix 58 | start_fix_span: 0.0 59 | # Period at the end of the spline to fix 60 | final_fix_span: -1.0 61 | # Steps to reassociate 62 | reassociate_steps: 2 63 | # Deskew choice, 0 for IMU prop, 1 for SPLINE, 1 for before and 1 for after 64 | deskew_method: [ 0, 0 ] 65 | # Rate of doing reassociation 66 | reassoc_rate: 0 67 | # Use ceres 68 | use_ceres: 0 69 | 70 | # Downsampling grid size, chose -1 for no downsampling 71 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf 72 | assoc_spacing: 0.4 73 | surfel_map_depth: 16 74 | surfel_min_point: 10 75 | surfel_min_depth: 1 76 | surfel_query_depth: 7 # Equal to Dmax + 1. Dmax is defined in the paper 77 | surfel_intsect_rad: 0.1 # r in the the paper 78 | surfel_min_plnrty: 0.7 79 | use_ufm: 1 80 | 81 | # Distance to surfel and min score thresholds 82 | dis_to_surfel_max: 0.5 83 | score_min: 0.0 84 | 85 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 86 | ds_rate: [ 87 | # 8 88 | # , 89 | 2 90 | ] 91 | 92 | # Number of pointclouds to merge into one 93 | sweep_len: 1 94 | 95 | # Threshold to add new keyframe 96 | kf_min_dis: 1.0 97 | kf_min_angle: 20 98 | refine_kf: 0 99 | 100 | # Optimization parameters 101 | lidar_loss_thres: -10.0 102 | imu_loss_thres: -1.0 103 | 104 | # Solver: 105 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 106 | linSolver: "snc" 107 | # Trust region strategy: "lm", "dogleg" 108 | trustRegType: "lm" 109 | # Linear Algebra library used: "eigen", "lapack", "cuda" 110 | linAlgbLib: "eigen" 111 | max_solve_time: 0.3 112 | max_iterations: 30 113 | ensure_real_time: 0 114 | find_factor_cost: 1 115 | fit_spline: 0 116 | # Damping factor for the LM method 117 | lambda: 1.0 118 | 119 | # Sensors fused 120 | fuse_lidar: 1 121 | fuse_imu: 1 122 | fuse_poseprop: 0 123 | fuse_velprop: 0 124 | fuse_marg: 1 125 | 126 | regularize_imu: 0 127 | imu_init_time: 0.5 128 | lite_redeskew: 1 129 | max_outer_iters: 3 130 | max_lidar_factor: 8000 # Maximum number of lidar factors 131 | dj_thres: 0.0 132 | dx_thres: 0.5 133 | 134 | # Loop closure parameters 135 | loop_en: 1 136 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 137 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 138 | 139 | icpMaxIter: 100 # Maximum iterations for ICP 140 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 141 | histDis: 15 # Maximum correspondence distance for icp 142 | 143 | # Bundle adjustment params 144 | rib_edge: 100 145 | odom_q_noise: 0.1 146 | odom_p_noise: 0.1 147 | loop_weight: 0.05 148 | 149 | # Number of optimizations before quitting 150 | publish_map: 0 151 | 152 | # Terminal outputs 153 | show_report: 1 154 | show_preop_times: 1 155 | show_lioop_times: 1 156 | 157 | # Log directory 158 | log_dir: "/home/tmn/slict_log/atv" 159 | 160 | # Configurations for relocalization 161 | relocalization: 162 | 163 | # pre process 164 | ds_size: 0.25 165 | maximum_corner_num: 100 166 | 167 | # key points 168 | plane_detection_thre: 0.01 169 | plane_merge_normal_thre: 0.2 170 | voxel_size: 2.0 171 | voxel_init_num: 10 172 | proj_image_resolution: 0.5 173 | proj_dis_min: 0 174 | proj_dis_max: 5 175 | corner_thre: 10 176 | 177 | # std descriptor 178 | descriptor_near_num: 10 179 | descriptor_min_len: 2 180 | descriptor_max_len: 50 181 | non_max_suppression_radius: 2 182 | std_side_resolution: 0.2 183 | 184 | # candidate search 185 | skip_near_num: 100 186 | candidate_num: 50 187 | sub_frame_num: 1 188 | vertex_diff_threshold: 0.5 189 | rough_dis_threshold: 0.05 190 | normal_threshold: 0.2 191 | dis_threshold: 0.5 192 | icp_threshold: 0.4 193 | 194 | # Number of keyframe to merge for each reloc check 195 | kf_merged: 1 196 | # For guestimation filtering 197 | refine_reloc_tf: 1 198 | reloc_thres: 2 199 | reloc_timespan: -1.0 200 | pose_percentile_thres: 0.7 201 | priormap_viz_res: 0.2 202 | marginalize_new_points: 0 203 | ioa_max_iter: 20 -------------------------------------------------------------------------------- /config/mulran.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/os_cloud_node/points" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Ouster 16 | -1.0, 0.0, 0.0, -0.0063, 17 | 0.0, -1.0, 0.0, 0.0118, 18 | 0.0, 0.0, 1.0, 0.0285, 19 | 0.0, 0.0, 0.0, 1.0000 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 1.0 24 | 25 | # IMU topic 26 | imu_topic: "/imu" 27 | 28 | # Gravity constant 29 | GRAV: 9.82 30 | 31 | # IMU noises 32 | GYR_N: 4 33 | GYR_W: 8 34 | ACC_N: 2 35 | ACC_W: 8 36 | 37 | # Position prior weight 38 | POSE_N: 20 39 | 40 | # Noise of velocity propagation 41 | VEL_N: 10 42 | 43 | # Weightage of a basic lidar factor 44 | lidar_weight: 2 45 | 46 | # Bias bounds 47 | BG_BOUND: [ 0.1, 0.1, 0.2] 48 | BA_BOUND: [ 0.3, 0.3, 0.4] 49 | 50 | # Number of packets to process at time 51 | WINDOW_SIZE: 3 52 | # Number of substates to give to each packet 53 | N_SUB_SEG: 4 54 | # Spline order 55 | SPLINE_N: 4 56 | # Knot length 57 | deltaT: 0.01 58 | # Period at the beginning of the spline to fix 59 | start_fix_span: 0.0 60 | # Period at the end of the spline to fix 61 | final_fix_span: -1.0 62 | # Steps to reassociate 63 | reassociate_steps: 3 64 | # Deskew choice, 0 for IMU prop, 1 for SPLINE, 1 for before and 1 for after 65 | deskew_method: [ 0, 0 ] 66 | # Rate of doing reassociation 67 | reassoc_rate: 0 68 | # Use ceres 69 | use_ceres: 0 70 | 71 | # Downsampling grid size, chose -1 for no downsampling 72 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf 73 | assoc_spacing: 0.4 74 | surfel_map_depth: 16 75 | surfel_min_point: 6 76 | surfel_min_depth: 1 77 | surfel_query_depth: 6 # Equal to Dmax + 1. Dmax is defined in the paper 78 | surfel_intsect_rad: 0.2 # r in the the paper 79 | surfel_min_plnrty: 0.2 80 | use_ufm: 1 81 | 82 | # Distance to surfel and min score thresholds 83 | dis_to_surfel_max: 0.3 84 | score_min: 0.0 85 | 86 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 87 | ds_rate: [ 88 | # 8 89 | # , 90 | 2 91 | ] 92 | 93 | # Number of pointclouds to merge into one 94 | sweep_len: 1 95 | 96 | # Threshold to add new keyframe 97 | kf_min_dis: 1.0 98 | kf_min_angle: 20 99 | refine_kf: 0 100 | 101 | # Optimization parameters 102 | lidar_loss_thres: -10.0 103 | imu_loss_thres: -1.0 104 | 105 | # Solver: 106 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 107 | linSolver: "snc" 108 | # Trust region strategy: "lm", "dogleg" 109 | trustRegType: "lm" 110 | # Linear Algebra library used: "eigen", "lapack", "cuda" 111 | linAlgbLib: "eigen" 112 | max_solve_time: 0.3 113 | max_iterations: 30 114 | ensure_real_time: 0 115 | find_factor_cost: 1 116 | fit_spline: 0 117 | # Damping factor for the LM method 118 | lambda: 1.0 119 | 120 | # Sensors fused 121 | fuse_lidar: 1 122 | fuse_imu: 1 123 | fuse_poseprop: 0 124 | fuse_velprop: 0 125 | fuse_marg: 1 126 | 127 | regularize_imu: 1 128 | imu_init_time: 0.5 129 | lite_redeskew: 0 130 | max_outer_iters: 3 131 | max_lidar_factor: 4000 # Maximum number of lidar factors 132 | dj_thres: 0.0 133 | dx_thres: 0.5 134 | 135 | # Loop closure parameters 136 | loop_en: 1 137 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 138 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 139 | 140 | icpMaxIter: 100 # Maximum iterations for ICP 141 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 142 | histDis: 15 # Maximum correspondence distance for icp 143 | 144 | # Bundle adjustment params 145 | rib_edge: 100 146 | odom_q_noise: 0.1 147 | odom_p_noise: 0.1 148 | loop_weight: 0.05 149 | 150 | # Number of optimizations before quitting 151 | publish_map: 0 152 | 153 | # Terminal outputs 154 | show_report: 1 155 | show_preop_times: 1 156 | show_lioop_times: 1 157 | 158 | # Log directory 159 | log_dir: "/home/tmn/slict_log/atv" 160 | 161 | # Configurations for relocalization 162 | relocalization: 163 | 164 | # pre process 165 | ds_size: 0.25 166 | maximum_corner_num: 100 167 | 168 | # key points 169 | plane_detection_thre: 0.01 170 | plane_merge_normal_thre: 0.2 171 | voxel_size: 2.0 172 | voxel_init_num: 10 173 | proj_image_resolution: 0.5 174 | proj_dis_min: 0 175 | proj_dis_max: 5 176 | corner_thre: 10 177 | 178 | # std descriptor 179 | descriptor_near_num: 10 180 | descriptor_min_len: 2 181 | descriptor_max_len: 50 182 | non_max_suppression_radius: 2 183 | std_side_resolution: 0.2 184 | 185 | # candidate search 186 | skip_near_num: 100 187 | candidate_num: 50 188 | sub_frame_num: 1 189 | vertex_diff_threshold: 0.5 190 | rough_dis_threshold: 0.05 191 | normal_threshold: 0.2 192 | dis_threshold: 0.5 193 | icp_threshold: 0.4 194 | 195 | # Number of keyframe to merge for each reloc check 196 | kf_merged: 1 197 | # For guestimation filtering 198 | refine_reloc_tf: 1 199 | reloc_thres: 2 200 | reloc_timespan: -1.0 201 | pose_percentile_thres: 0.7 202 | priormap_viz_res: 0.2 203 | marginalize_new_points: 0 204 | ioa_max_iter: 20 -------------------------------------------------------------------------------- /config/oxford.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | /os1_cloud_node/points 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Horizontal 16 | -1.0, 0.0, 0.0, -0.0063, 17 | 0.0, -1.0, 0.0, 0.0118, 18 | 0.0, 0.0, 1.0, 0.0285, 19 | 0.0, 0.0, 0.0, 1.0000 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 1.0 24 | 25 | # IMU topic 26 | imu_topic: "/os1_cloud_node/imu" 27 | 28 | # Gravity constant 29 | GRAV: 9.81007 30 | 31 | # IMU noises 32 | GYR_N: 2 33 | GYR_W: 4 34 | ACC_N: 0.2 35 | ACC_W: 4 36 | 37 | # Position prior weight 38 | POSE_N: 20 39 | 40 | # Noise of velocity propagation 41 | VEL_N: 10 42 | 43 | # Weightage of a basic lidar factor 44 | lidar_weight: 4 45 | 46 | # Bias bounds 47 | BG_BOUND: [ 0.3, 0.3, 0.4] 48 | BA_BOUND: [ 0.3, 0.3, 0.4] 49 | 50 | # Number of packets to process at time 51 | WINDOW_SIZE: 3 52 | # Number of substates to give to each packet 53 | N_SUB_SEG: 4 54 | # Spline order 55 | SPLINE_N: 4 56 | # Knot length 57 | deltaT: 0.02 58 | # Period at the beginning of the spline to fix 59 | start_fix_span: 0.0 60 | # Period at the end of the spline to fix 61 | final_fix_span: -1.0 62 | # Steps to reassociate 63 | reassociate_steps: 2 64 | # Deskew choice, 0 for IMU prop, 1 for SPLINE, 1 for before and 1 for after 65 | deskew_method: [ 0, 0 ] 66 | # Rate of doing reassociation 67 | reassoc_rate: 0 68 | # Use ceres 69 | use_ceres: 0 70 | 71 | # Downsampling grid size, chose -1 for no downsampling 72 | leaf_size: 0.1 # Downsample scale of input pointcloud, also the scale of ufomap leaf 73 | assoc_spacing: 0.8 74 | surfel_map_depth: 16 75 | surfel_min_point: 6 76 | surfel_min_depth: 1 77 | surfel_query_depth: 7 # Equal to Dmax + 1. Dmax is defined in the paper 78 | surfel_intsect_rad: 0.1 # r in the the paper 79 | surfel_min_plnrty: 0.2 80 | use_ufm: 1 81 | 82 | # Distance to surfel and min score thresholds 83 | dis_to_surfel_max: 0.5 84 | score_min: 0.0 85 | 86 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 87 | ds_rate: [ 88 | 4 89 | ] 90 | 91 | # Number of pointclouds to merge into one 92 | sweep_len: 1 93 | 94 | # Threshold to add new keyframe 95 | kf_min_dis: 4.0 96 | kf_min_angle: 20 97 | refine_kf: 0 98 | 99 | # Optimization parameters 100 | lidar_loss_thres: -10.0 101 | imu_loss_thres: -1.0 102 | 103 | # Solver: 104 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 105 | linSolver: "snc" 106 | # Trust region strategy: "lm", "dogleg" 107 | trustRegType: "lm" 108 | # Linear Algebra library used: "eigen", "lapack", "cuda" 109 | linAlgbLib: "eigen" 110 | max_solve_time: 0.3 111 | max_iterations: 30 112 | ensure_real_time: 0 113 | find_factor_cost: 1 114 | fit_spline: 0 115 | # Damping factor for the LM method 116 | lambda: 1.0 117 | 118 | # Sensors fused 119 | fuse_lidar: 1 120 | fuse_imu: 1 121 | fuse_poseprop: 0 122 | fuse_velprop: 0 123 | 124 | regularize_imu: 0 125 | imu_init_time: 0.5 126 | lite_redeskew: 0 127 | max_outer_iters: 3 128 | max_lidar_factor: 4000 # Maximum number of lidar factors 129 | dj_thres: 0.0 130 | dx_thres: 0.5 131 | 132 | # Loop closure parameters 133 | loop_en: 1 134 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 135 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 136 | 137 | icpMaxIter: 100 # Maximum iterations for ICP 138 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 139 | histDis: 15 # Maximum correspondence distance for icp 140 | 141 | # Bundle adjustment params 142 | rib_edge: 100 143 | odom_q_noise: 0.1 144 | odom_p_noise: 0.1 145 | loop_weight: 0.05 146 | 147 | # Number of optimizations before quitting 148 | publish_map: 0 149 | 150 | # Terminal outputs 151 | show_report: 1 152 | show_preop_times: 1 153 | show_lioop_times: 1 154 | 155 | # Log directory 156 | log_dir: "/home/tmn/slict_log/oxford" 157 | 158 | # Configurations for relocalization 159 | relocalization: 160 | 161 | # pre process 162 | ds_size: 0.25 163 | maximum_corner_num: 100 164 | 165 | # key points 166 | plane_detection_thre: 0.01 167 | plane_merge_normal_thre: 0.2 168 | voxel_size: 2.0 169 | voxel_init_num: 10 170 | proj_image_resolution: 0.5 171 | proj_dis_min: 0 172 | proj_dis_max: 5 173 | corner_thre: 10 174 | 175 | # std descriptor 176 | descriptor_near_num: 10 177 | descriptor_min_len: 2 178 | descriptor_max_len: 50 179 | non_max_suppression_radius: 2 180 | std_side_resolution: 0.2 181 | 182 | # candidate search 183 | skip_near_num: 100 184 | candidate_num: 50 185 | sub_frame_num: 1 186 | vertex_diff_threshold: 0.5 187 | rough_dis_threshold: 0.05 188 | normal_threshold: 0.2 189 | dis_threshold: 0.5 190 | icp_threshold: 0.4 191 | 192 | # Number of keyframe to merge for each reloc check 193 | kf_merged: 1 194 | # For guestimation filtering 195 | refine_reloc_tf: 1 196 | reloc_thres: 2 197 | reloc_timespan: -1.0 198 | pose_percentile_thres: 0.7 199 | priormap_viz_res: 0.2 200 | marginalize_new_points: 0 201 | ioa_max_iter: 40 -------------------------------------------------------------------------------- /config/stillwh.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 0 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | /os_cloud_node/points 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | # Horizontal 16 | 1.0, 0.0, 0.0, -0.0063, 17 | 0.0, 1.0, 0.0, 0.0118, 18 | 0.0, 0.0, 1.0, 0.0285, 19 | 0.0, 0.0, 0.0, 1.0000 20 | ] 21 | 22 | # Minimum lidar range to admit 23 | min_range: 2.0 24 | 25 | # IMU topic 26 | imu_topic: "/os_cloud_node/imu" 27 | 28 | # Gravity constant 29 | GRAV: 9.81007 30 | 31 | # IMU noises 32 | GYR_N: 5.30298029414e-3 33 | GYR_W: 4.0e-06 34 | ACC_N: 3.18433768624e-2 35 | ACC_W: 10.6e-5 36 | 37 | # Weightage of a basic lidar factor 38 | lidar_weight: 1.0 39 | 40 | # Number of packets to process at time 41 | WINDOW_SIZE: 3 42 | # Number of substates to give to each packet 43 | N_SUB_SEG: 2 44 | 45 | # Downsampling grid size, chose -1 for no downsampling 46 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leafs 47 | assoc_spacing: 0.2 48 | surfel_map_depth: 16 49 | surfel_min_point: 8 50 | surfel_min_depth: 1 51 | surfel_query_depth: 5 # Equal to Dmax + 1. Dmax is defined in the paper 52 | surfel_intsect_rad: 0.2 # r in the the paper 53 | surfel_min_plnrty: 0.4 54 | 55 | # Distance to surfel and min score thresholds 56 | dis_to_surfel_max: 0.3 57 | score_min: 0.0 58 | 59 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 60 | ds_rate: [ 61 | 4 62 | ] 63 | 64 | # Number of pointclouds to merge into one 65 | sweep_len: 1 66 | 67 | # Threshold to add new keyframe 68 | kf_min_dis: 1.0 69 | kf_min_angle: 10 70 | 71 | # Optimization parameters 72 | lidar_loss_thres: 10.0 73 | 74 | # Solver: 75 | # Trust region strategy: "lm", "dogleg" 76 | trustRegType: "lm" 77 | # Linear Algebra library used: "eigen", "lapack", "cuda" 78 | linAlgbLib: "cuda" 79 | max_solve_time: 0.3 80 | max_iterations: 40 81 | ensure_real_time: 0 82 | 83 | # Sensors fused 84 | fuse_lidar: 1 85 | fuse_imu: 1 86 | fuse_poseprop: 0 87 | fuse_velprop: 0 88 | 89 | regularize_imu: 1 90 | imu_init_time: 1.0 91 | max_outer_iters: 1 92 | max_lidar_factor: 4000 # Maximum number of lidar factors 93 | 94 | # Loop closure parameters 95 | loop_en: 1 # Enable loop closure 96 | loop_kf_nbr: 10 # Number of neighbour to check for loop closure 97 | loop_time_mindiff: 10 # Only check for loop when keyframes have this much difference 98 | 99 | icpMaxIter: 100 # Maximum iterations for ICP 100 | icpFitnessThres: 0.3 # Fitness threshold for ICP check 101 | histDis: 15 # Maximum correspondence distance for icp 102 | ioa_max_iter: 10 # Maximum iterations of the ioa method 103 | ioa_DJ_end: 0.001 # Terminate ioa when DJ is below this 104 | 105 | # Bundle adjustment params 106 | rib_edge: 5 107 | odom_q_noise: 0.1 108 | odom_p_noise: 0.1 109 | loop_weight: 0.1 110 | 111 | # Number of optimizations before quitting 112 | debug_exit: -1 113 | publish_map: 0 114 | 115 | # Log directory 116 | log_dir: "/home/tmn/slict_log/stillwh" 117 | 118 | 119 | relocalization: 120 | 121 | # pre process 122 | ds_size: 0.25 123 | maximum_corner_num: 100 124 | 125 | # key points 126 | plane_detection_thre: 0.01 127 | plane_merge_normal_thre: 0.2 128 | voxel_size: 2.0 129 | voxel_init_num: 10 130 | proj_image_resolution: 0.5 131 | proj_dis_min: 0 132 | proj_dis_max: 5 133 | corner_thre: 10 134 | 135 | # std descriptor 136 | descriptor_near_num: 10 137 | descriptor_min_len: 2 138 | descriptor_max_len: 50 139 | non_max_suppression_radius: 2 140 | std_side_resolution: 0.2 141 | 142 | # candidate search 143 | skip_near_num: 100 144 | candidate_num: 50 145 | sub_frame_num: 1 146 | vertex_diff_threshold: 0.5 147 | rough_dis_threshold: 0.01 148 | normal_threshold: 0.2 149 | dis_threshold: 0.5 150 | icp_threshold: 0.4 151 | 152 | # For guestimation filtering 153 | reloc_thres: 20 154 | reloc_timespan: 10.0 155 | pose_percentile_thres: 0.7 156 | priormap_viz_res: 0.05 157 | 158 | -------------------------------------------------------------------------------- /config/urbanloco_hk.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/velodyne_points_0_ouster" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 0, -1, 0, 0.00, 15 | 1, 0, 0, 0.00, 16 | 0, 0, 1, -0.28, 17 | 0, 0, 0, 1.00] 18 | 19 | # Minimum lidar range to admit 20 | min_range: 1.0 21 | 22 | # IMU topic 23 | imu_topic: "/imu/data" 24 | 25 | # Gravity constant 26 | GRAV: 9.82 27 | 28 | # IMU noises 29 | GYR_N: 2 30 | GYR_W: 4 31 | ACC_N: 0.2 32 | ACC_W: 4 33 | 34 | # Position prior weight 35 | POSE_N: 20 36 | 37 | # Noise of velocity propagation 38 | VEL_N: 10 39 | 40 | # Weightage of a basic lidar factor 41 | lidar_weight: 2 42 | 43 | # Bias bounds 44 | BG_BOUND: [ 0.1, 0.1, 0.2] 45 | BA_BOUND: [ 0.3, 0.3, 0.4] 46 | 47 | # Number of packets to process at time 48 | WINDOW_SIZE: 3 49 | # Number of substates to give to each packet 50 | N_SUB_SEG: 16 51 | # Spline order 52 | SPLINE_N: 4 53 | # Knot length 54 | deltaT: 0.01 55 | # Period at the beginning of the spline to fix 56 | start_fix_span: 0.0 57 | # Period at the end of the spline to fix 58 | final_fix_span: -1.0 59 | # Steps to reassociate 60 | reassociate_steps: 2 61 | # Deskew choice, 0 for IMU prop, 1 for SPLINE, 1 for before and 1 for after 62 | deskew_method: [ 0, 0 ] 63 | # Rate of doing reassociation 64 | reassoc_rate: 0 65 | # Use ceres 66 | use_ceres: 0 67 | 68 | # Downsampling grid size, chose -1 for no downsampling 69 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf 70 | assoc_spacing: 0.4 71 | surfel_map_depth: 16 72 | surfel_min_point: 5 73 | surfel_min_depth: 1 74 | surfel_query_depth: 7 # Equal to Dmax + 1. Dmax is defined in the paper 75 | surfel_intsect_rad: 0.1 # r in the the paper 76 | surfel_min_plnrty: 0.2 77 | use_ufm: 1 78 | 79 | # Distance to surfel and min score thresholds 80 | dis_to_surfel_max: 0.5 81 | score_min: 0.0 82 | 83 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 84 | ds_rate: [ 85 | # 8 86 | # , 87 | 1 88 | ] 89 | 90 | # Number of pointclouds to merge into one 91 | sweep_len: 1 92 | 93 | # Threshold to add new keyframe 94 | kf_min_dis: 1.0 95 | kf_min_angle: 20 96 | refine_kf: 0 97 | 98 | # Optimization parameters 99 | lidar_loss_thres: -10.0 100 | imu_loss_thres: -1.0 101 | 102 | # Solver: 103 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 104 | linSolver: "snc" 105 | # Trust region strategy: "lm", "dogleg" 106 | trustRegType: "lm" 107 | # Linear Algebra library used: "eigen", "lapack", "cuda" 108 | linAlgbLib: "eigen" 109 | max_solve_time: 0.3 110 | max_iterations: 30 111 | ensure_real_time: 0 112 | find_factor_cost: 1 113 | fit_spline: 0 114 | # Damping factor for the LM method 115 | lambda: 1.0 116 | 117 | # Sensors fused 118 | fuse_lidar: 1 119 | fuse_imu: 1 120 | fuse_poseprop: 0 121 | fuse_velprop: 0 122 | fuse_marg: 1 123 | 124 | regularize_imu: 0 125 | imu_init_time: 0.5 126 | lite_redeskew: 1 127 | max_outer_iters: 3 128 | max_lidar_factor: 8000 # Maximum number of lidar factors 129 | dj_thres: 0.0 130 | dx_thres: 0.5 131 | 132 | # Loop closure parameters 133 | loop_en: 1 134 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 135 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 136 | 137 | icpMaxIter: 100 # Maximum iterations for ICP 138 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 139 | histDis: 15 # Maximum correspondence distance for icp 140 | 141 | # Bundle adjustment params 142 | rib_edge: 100 143 | odom_q_noise: 0.1 144 | odom_p_noise: 0.1 145 | loop_weight: 0.05 146 | 147 | # Number of optimizations before quitting 148 | publish_map: 0 149 | 150 | # Terminal outputs 151 | show_report: 1 152 | show_preop_times: 1 153 | show_lioop_times: 1 154 | 155 | # Log directory 156 | log_dir: "/home/tmn/slict_log/atv" 157 | 158 | # Configurations for relocalization 159 | relocalization: 160 | 161 | # pre process 162 | ds_size: 0.25 163 | maximum_corner_num: 100 164 | 165 | # key points 166 | plane_detection_thre: 0.01 167 | plane_merge_normal_thre: 0.2 168 | voxel_size: 2.0 169 | voxel_init_num: 10 170 | proj_image_resolution: 0.5 171 | proj_dis_min: 0 172 | proj_dis_max: 5 173 | corner_thre: 10 174 | 175 | # std descriptor 176 | descriptor_near_num: 10 177 | descriptor_min_len: 2 178 | descriptor_max_len: 50 179 | non_max_suppression_radius: 2 180 | std_side_resolution: 0.2 181 | 182 | # candidate search 183 | skip_near_num: 100 184 | candidate_num: 50 185 | sub_frame_num: 1 186 | vertex_diff_threshold: 0.5 187 | rough_dis_threshold: 0.05 188 | normal_threshold: 0.2 189 | dis_threshold: 0.5 190 | icp_threshold: 0.4 191 | 192 | # Number of keyframe to merge for each reloc check 193 | kf_merged: 1 194 | # For guestimation filtering 195 | refine_reloc_tf: 1 196 | reloc_thres: 2 197 | reloc_timespan: -1.0 198 | pose_percentile_thres: 0.7 199 | priormap_viz_res: 0.2 200 | marginalize_new_points: 0 201 | ioa_max_iter: 20 -------------------------------------------------------------------------------- /config/whu.yaml: -------------------------------------------------------------------------------- 1 | # To inform the algorithm to exit automatically when no data is received 2 | autoexit: 1 3 | 4 | # Declare the lidar topics to merge 5 | lidar_topic: [ 6 | "/livox/lidar_ouster" 7 | ] 8 | 9 | merged_lidar_topic: "/merged_pc" 10 | 11 | # Lidar extrinsics: Transform of coordinates in lidar's frame to body 12 | # NOTE TO USER: The (4,4) entry of the extrinsics is used to declare 13 | # whether stamp is at beginning (1.000) or end (0.000) of scan 14 | lidar_extr: [ 15 | -0.0048858, -0.9997687, -0.0209424, 0.083, 16 | 0.9999720, -0.0047662, -0.0057583, 0.077, 17 | 0.0056571, -0.0209700, 0.9997641, 0.035, 18 | 0.0000000, 0.0000000, 0.0000000, 1.000 19 | ] 20 | 21 | # Minimum lidar range to admit 22 | min_range: 0.5 23 | 24 | # IMU topic 25 | imu_topic: "/imu0_mod" 26 | 27 | # Gravity constant 28 | GRAV: 9.82 29 | 30 | # IMU noises 31 | GYR_N: 20 32 | GYR_W: 10 33 | ACC_N: 1 34 | ACC_W: 10 35 | 36 | # Position prior weight 37 | POSE_N: 100 38 | 39 | # Noise of velocity propagation 40 | VEL_N: 10 41 | 42 | # Weightage of a basic lidar factor 43 | lidar_weight: 10 44 | 45 | # Bias bounds 46 | BG_BOUND: [ 0.1, 0.1, 0.2] 47 | BA_BOUND: [ 0.1, 0.1, 0.2] 48 | 49 | # Number of packets to process at time 50 | WINDOW_SIZE: 4 51 | # Number of substates to give to each packet 52 | N_SUB_SEG: 8 53 | # Spline order 54 | SPLINE_N: 4 55 | # Knot length 56 | deltaT: 0.05 57 | # Period at the beginning of the spline to fix 58 | start_fix_span: -0.0 59 | # Period at the end of the spline to fix 60 | final_fix_span: -1.0 61 | # Steps to reassociate 62 | reassociate_steps: 3 63 | 64 | # Downsampling grid size, chose -1 for no downsampling 65 | leaf_size: 0.05 # Downsample scale of input pointcloud, also the scale of ufomap leaf 66 | assoc_spacing: 0.8 67 | surfel_map_depth: 16 68 | surfel_min_point: 6 69 | surfel_min_depth: 1 70 | surfel_query_depth: 7 # Equal to Dmax + 1. Dmax is defined in the paper 71 | surfel_intsect_rad: 0.2 # r in the the paper 72 | surfel_min_plnrty: 0.6 73 | 74 | # Distance to surfel and min score thresholds 75 | dis_to_surfel_max: 0.3 76 | score_min: 0.0 77 | 78 | # Downsample coefficient of lidar scans, ds_rate = n means keep every n-th point. 79 | ds_rate: [ 80 | 1 81 | ] 82 | 83 | # Threshold to add new keyframe 84 | kf_min_dis: 0.5 85 | kf_min_angle: 10 86 | refine_kf: 0 87 | 88 | # Optimization parameters 89 | lidar_loss_thres: 10.0 90 | 91 | # Solver: 92 | # Linear solver: "dnc" "snc" "cgnr" "dschur" "sschur" "ischur" 93 | linSolver: "snc" 94 | # Trust region strategy: "lm", "dogleg" 95 | trustRegType: "lm" 96 | # Linear Algebra library used: "eigen", "lapack", "cuda" 97 | linAlgbLib: "eigen" 98 | max_solve_time: 0.3 99 | max_iterations: 30 100 | ensure_real_time: 0 101 | find_factor_cost: 0 102 | fit_spline: 0 103 | 104 | # Sensors fused 105 | fuse_lidar: 1 106 | fuse_imu: 1 107 | fuse_poseprop: 1 108 | fuse_velprop: 0 109 | 110 | regularize_imu: 1 111 | imu_init_time: 0.5 112 | max_outer_iters: 3 113 | max_lidar_factor: 4000 # Maximum number of lidar factors 114 | 115 | # Loop closure parameters 116 | loop_en: 1 117 | loop_kf_nbr: 20 # Number of neighbour to check for loop closure 118 | loop_kfid_mindiff: 20 # Only check for loop when keyframes have this much difference 119 | 120 | icpMaxIter: 100 # Maximum iterations for ICP 121 | icpFitnessThres: 0.6 # Fitness threshold for ICP check 122 | histDis: 15 # Maximum correspondence distance for icp 123 | 124 | # Bundle adjustment params 125 | rib_edge: 100 126 | odom_q_noise: 0.1 127 | odom_p_noise: 0.1 128 | loop_weight: 0.05 129 | 130 | # Whether to publish the map pc 131 | publish_map: 0 132 | 133 | # Log directory 134 | log_dir: "/home/tmn/slict_log/atv" 135 | 136 | # Configurations for relocalization 137 | relocalization: 138 | 139 | # pre process 140 | ds_size: 0.25 141 | maximum_corner_num: 100 142 | 143 | # key points 144 | plane_detection_thre: 0.01 145 | plane_merge_normal_thre: 0.2 146 | voxel_size: 2.0 147 | voxel_init_num: 10 148 | proj_image_resolution: 0.5 149 | proj_dis_min: 0 150 | proj_dis_max: 5 151 | corner_thre: 10 152 | 153 | # std descriptor 154 | descriptor_near_num: 10 155 | descriptor_min_len: 2 156 | descriptor_max_len: 50 157 | non_max_suppression_radius: 2 158 | std_side_resolution: 0.2 159 | 160 | # candidate search 161 | skip_near_num: 100 162 | candidate_num: 50 163 | sub_frame_num: 1 164 | vertex_diff_threshold: 0.5 165 | rough_dis_threshold: 0.01 166 | normal_threshold: 0.2 167 | dis_threshold: 0.5 168 | icp_threshold: 0.4 169 | 170 | # For guestimation filtering 171 | refine_reloc_tf: 0 172 | reloc_thres: 3 173 | reloc_timespan: 5.0 174 | pose_percentile_thres: 0.7 175 | priormap_viz_res: 0.2 176 | marginalize_new_points: 0 177 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic-perception 2 | 3 | ENV CERES_VERSION="2.1.0" 4 | ENV PCL_VERSION="1.10.0" 5 | # ENV USER="root" 6 | ENV CATKIN_WS=/root/catkin_ws 7 | ENV DATA_PATH=/root/dataset 8 | ENV LIBRARY=/root/library 9 | 10 | RUN mkdir -p $CATKIN_WS/src 11 | RUN mkdir -p $DATA_PATH 12 | RUN mkdir -p $LIBRARY 13 | 14 | # set up processor number used to compile library 15 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \ 16 | # Install dependencies 17 | apt-get update && apt-get install -y \ 18 | git \ 19 | cmake \ 20 | nano \ 21 | ros-${ROS_DISTRO}-tf-conversions \ 22 | libatlas-base-dev \ 23 | libeigen3-dev \ 24 | libgoogle-glog-dev \ 25 | libsuitesparse-dev \ 26 | python3-wstool \ 27 | python3-catkin-tools \ 28 | && \ 29 | # Build and install Ceres 30 | cd $LIBRARY && \ 31 | git clone https://ceres-solver.googlesource.com/ceres-solver && \ 32 | cd ceres-solver && \ 33 | git checkout ${CERES_VERSION} && \ 34 | mkdir build && cd build && \ 35 | cmake .. && \ 36 | make -j${USE_PROC} install && \ 37 | cd ../.. && \ 38 | rm -rf ./ceres-solver && \ 39 | mkdir -p $CATKIN_WS/src/ufomap && \ 40 | mkdir -p $CATKIN_WS/src/slict && \ 41 | mkdir -p $CATKIN_WS/src/livox_ros_driver && \ 42 | cd 43 | 44 | # Clone UFOMap and checkout the devel_surfel branch 45 | RUN git clone https://github.com/brytsknguyen/ufomap $CATKIN_WS/src/ufomap && \ 46 | cd $CATKIN_WS/src/ufomap && git checkout devel_surfel && cd 47 | 48 | WORKDIR $CATKIN_WS 49 | RUN /bin/bash -c 'source /opt/ros/noetic/setup.bash; cd ${CATKIN_WS}; catkin build' 50 | 51 | # Clone and checkout the livox_ros_driver 52 | RUN git clone https://github.com/brytsknguyen/livox_ros_driver $CATKIN_WS/src/livox_ros_driver 53 | 54 | WORKDIR $CATKIN_WS 55 | RUN /bin/bash -c 'source /opt/ros/noetic/setup.bash; cd ${CATKIN_WS}; catkin build' 56 | 57 | # Copy slict code 58 | COPY ./ $CATKIN_WS/src/slict 59 | 60 | # Build the catkin workspace 61 | WORKDIR $CATKIN_WS 62 | RUN /bin/bash -c 'source /opt/ros/noetic/setup.bash; cd ${CATKIN_WS}; catkin build' && \ 63 | sed -i '/exec "$@"/i source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh -------------------------------------------------------------------------------- /docker/Makefile: -------------------------------------------------------------------------------- 1 | all: help 2 | 3 | help: 4 | @echo "" 5 | @echo "-- Help Menu" 6 | @echo "" 7 | @echo " 1. make build - build all images" 8 | # @echo " 1. make pull - pull all images" 9 | @echo " 1. make clean - remove all images" 10 | @echo "" 11 | 12 | build: 13 | @docker build --squash --network=host --tag slict-noetic-focal -f ./Dockerfile .. 14 | 15 | clean: 16 | @docker rmi -f slict-noetic-focal 17 | -------------------------------------------------------------------------------- /docker/run_fuPo_handheld.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Kill all child processes when this script is killed 4 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 5 | 6 | DATA_PATH=${1:-/media/tmn/mySataSSD3/FusionPortable/} 7 | DOCKER_IMG=brytsknguyen/slict-noetic-focal 8 | 9 | # Pull the image from docker hub 10 | docker pull $DOCKER_IMG 11 | 12 | PKG_DIR=$(rospack find slict) 13 | 14 | # Launch rviz in background to give terminal to slict 15 | (rviz -d ${PKG_DIR}/launch/fuPo_handheld.rviz) & 16 | 17 | # Launch slict 18 | docker run \ 19 | -it \ 20 | --rm \ 21 | --net=host \ 22 | -v ${PKG_DIR}:/root/catkin_ws/src/slict \ 23 | -v ${DATA_PATH}:/root/dataset/ \ 24 | -e USER=root \ 25 | $DOCKER_IMG \ 26 | /bin/bash -c \ 27 | "cd /root/catkin_ws/; \ 28 | catkin build; \ 29 | source devel/setup.bash; \ 30 | roslaunch slict run_fuPo_handheld.launch data_path:=/root/dataset/ exp_log_dir:=/root/slict_logs/fuPo_handheld" -------------------------------------------------------------------------------- /docker/run_mcdviral.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Kill all child processes when this script is killed 4 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 5 | 6 | DATA_PATH=${1:-/media/tmn/mySataSSD1/MCDVIRAL/} 7 | DOCKER_IMG=brytsknguyen/slict-noetic-focal 8 | 9 | # Pull the image from docker hub 10 | docker pull $DOCKER_IMG 11 | 12 | PKG_DIR=$(rospack find slict) 13 | 14 | # Launch rviz in background to give terminal to slict 15 | (rviz -d ${PKG_DIR}/launch/mcdviral_atv.rviz) & 16 | 17 | # Launch slict 18 | docker run -it --rm --net=host \ 19 | -v ${PKG_DIR}:/root/catkin_ws/src/slict \ 20 | -v ${DATA_PATH}:/root/dataset/ \ 21 | -e USER=root \ 22 | $DOCKER_IMG \ 23 | /bin/bash -c "cd /root/catkin_ws/; \ 24 | catkin build; \ 25 | source devel/setup.bash; \ 26 | roslaunch slict run_mcdviral.launch data_path:=/root/dataset/ exp_log_dir:=/root/slict_logs/mcdviral" -------------------------------------------------------------------------------- /docker/run_ntuviral.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Kill all child processes when this script is killed 4 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 5 | 6 | DATA_PATH=${1:-/media/tmn/mySataSSD2/DATASETS/NTU_VIRAL} 7 | DOCKER_IMG=brytsknguyen/slict-noetic-focal 8 | 9 | # Pull the image from docker hub 10 | docker pull $DOCKER_IMG 11 | 12 | PKG_DIR=$(rospack find slict) 13 | 14 | # Launch rviz in background to give terminal to slict 15 | (rviz -d ${PKG_DIR}/launch/ntuviral.rviz) & 16 | 17 | # Launch slict 18 | docker run -it --rm --net=host \ 19 | -v ${PKG_DIR}:/root/catkin_ws/src/slict \ 20 | -v ${DATA_PATH}:/root/dataset/ \ 21 | -e USER=root \ 22 | $DOCKER_IMG \ 23 | /bin/bash -c "cd /root/catkin_ws/; \ 24 | catkin build; \ 25 | source devel/setup.bash; \ 26 | roslaunch slict run_ntuviral.launch data_path:=/root/dataset/ exp_log_dir:=/root/slict_logs/ntuviral" -------------------------------------------------------------------------------- /docker/run_oxford.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Kill all child processes when this script is killed 4 | trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT 5 | 6 | DATA_PATH=${1:-/media/tmn/mySataSSD1/NewerCollegeDataset/} 7 | DOCKER_IMG=brytsknguyen/slict-noetic-focal 8 | 9 | # Pull the image from docker hub 10 | docker pull $DOCKER_IMG 11 | 12 | PKG_DIR=$(rospack find slict) 13 | 14 | # Launch rviz in background to give terminal to slict 15 | (rviz -d ${PKG_DIR}/launch/oxford.rviz) & 16 | 17 | # Launch slict 18 | docker run -it --rm --net=host \ 19 | -v ${PKG_DIR}:/root/catkin_ws/src/slict \ 20 | -v ${DATA_PATH}:/root/dataset/ \ 21 | -e USER=root \ 22 | $DOCKER_IMG \ 23 | /bin/bash -c "cd /root/catkin_ws/; \ 24 | catkin build; \ 25 | source devel/setup.bash; \ 26 | roslaunch slict run_oxford.launch data_path:=/root/dataset/ exp_log_dir:=/root/slict_logs/oxford" -------------------------------------------------------------------------------- /docs/Course.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/Course.png -------------------------------------------------------------------------------- /docs/GM_20230126_SLICT.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/GM_20230126_SLICT.pdf -------------------------------------------------------------------------------- /docs/ba-dum-tsss.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/ba-dum-tsss.gif -------------------------------------------------------------------------------- /docs/example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/example.png -------------------------------------------------------------------------------- /docs/slam_backbone.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/slam_backbone.png -------------------------------------------------------------------------------- /docs/thumbnail.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/brytsknguyen/slict/17a2dfaad58c9765fbc9a78a868827ed55ada270/docs/thumbnail.png -------------------------------------------------------------------------------- /include/PoseLocalParameterization.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * School of EEE 6 | * Nanyang Technological Univertsity, Singapore 7 | * 8 | * For more information please see . 9 | * or . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * slict is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * slict is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with slict. If not, see . 25 | */ 26 | 27 | // 28 | // Created by Thien-Minh Nguyen on 01/08/22. 29 | // 30 | 31 | #pragma once 32 | 33 | #include 34 | #include 35 | #include "utility.h" 36 | 37 | class PoseLocalParameterization : public ceres::LocalParameterization 38 | { 39 | bool Plus(const double *x, const double *delta, double *x_plus_delta) const 40 | { 41 | Eigen::Map _p(x); 42 | Eigen::Map _q(x + 3); 43 | 44 | Eigen::Map dp(delta); 45 | 46 | Eigen::Quaterniond dq = Util::QExp(Eigen::Map(delta + 3)); 47 | 48 | Eigen::Map p(x_plus_delta); 49 | Eigen::Map q(x_plus_delta + 3); 50 | 51 | p = _p + dp; 52 | q = (_q * dq).normalized(); 53 | 54 | // Eigen::Vector3d euler(Utility::R2ypr(q.toRotationMatrix())); 55 | // euler.y() = 30.0*euler.y()/std::max(30.0, fabs(euler.y())); 56 | // euler.z() = 30.0*euler.y()/std::max(30.0, fabs(euler.z())); 57 | // q = Quaterniond(Utility::ypr2R(euler.x(), euler.y(), euler.z())); 58 | 59 | return true; 60 | } 61 | 62 | virtual bool ComputeJacobian(const double *x, double *jacobian) const 63 | { 64 | Eigen::Map> j(jacobian); 65 | j.topRows<6>().setIdentity(); 66 | j.bottomRows<1>().setZero(); 67 | 68 | return true; 69 | } 70 | 71 | virtual int GlobalSize() const { return 7; }; 72 | virtual int LocalSize() const { return 6; }; 73 | }; 74 | -------------------------------------------------------------------------------- /include/basalt/spline/posesplinex.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | // Basalt 6 | #include "basalt/spline/se3_spline.h" 7 | #include "basalt/spline/ceres_spline_helper.h" 8 | #include "basalt/spline/ceres_local_param.hpp" 9 | 10 | using namespace std; 11 | using namespace Eigen; 12 | 13 | // Shortened typedef matching character length of Vector3d and Matrix3d 14 | typedef Eigen::Quaterniond Quaternd; 15 | typedef Eigen::Quaterniond Quaternf; 16 | 17 | 18 | // Shorthand for sophus objects 19 | typedef Sophus::SO3 SO3d; 20 | typedef Sophus::SE3 SE3d; 21 | 22 | 23 | /* #region "Dynamic order" spline -----------------------------------------------------------------------------------*/ 24 | typedef basalt::Se3Spline<4> PoseSpline4; 25 | typedef basalt::Se3Spline<5> PoseSpline5; 26 | typedef basalt::Se3Spline<6> PoseSpline6; 27 | typedef basalt::Se3Spline<7> PoseSpline7; 28 | typedef basalt::Se3Spline<8> PoseSpline8; 29 | typedef basalt::Se3Spline<9> PoseSpline9; 30 | 31 | // Function to set the sline 32 | #define SPLINE_SET(SF, ...) \ 33 | switch(X) \ 34 | { \ 35 | case(4): traj4.SF(__VA_ARGS__); break; \ 36 | case(5): traj5.SF(__VA_ARGS__); break; \ 37 | case(6): traj6.SF(__VA_ARGS__); break; \ 38 | case(7): traj7.SF(__VA_ARGS__); break; \ 39 | case(8): traj8.SF(__VA_ARGS__); break; \ 40 | case(9): traj9.SF(__VA_ARGS__); break; \ 41 | }; \ 42 | 43 | // Function to query the sline 44 | #define SPLINE_GET(SF, ...) \ 45 | switch(X) \ 46 | { \ 47 | case(4): return traj4.SF(__VA_ARGS__); \ 48 | case(5): return traj5.SF(__VA_ARGS__); \ 49 | case(6): return traj6.SF(__VA_ARGS__); \ 50 | case(7): return traj7.SF(__VA_ARGS__); \ 51 | case(8): return traj8.SF(__VA_ARGS__); \ 52 | case(9): return traj9.SF(__VA_ARGS__); \ 53 | }; \ 54 | 55 | class PoseSplineX 56 | { 57 | 58 | private: 59 | PoseSpline4 traj4; 60 | PoseSpline5 traj5; 61 | PoseSpline6 traj6; 62 | PoseSpline7 traj7; 63 | PoseSpline8 traj8; 64 | PoseSpline9 traj9; 65 | int X; 66 | 67 | public: 68 | 69 | PoseSplineX(int X_, double dt) 70 | : 71 | X(X_), 72 | traj4(PoseSpline4(dt)), 73 | traj5(PoseSpline5(dt)), 74 | traj6(PoseSpline6(dt)), 75 | traj7(PoseSpline7(dt)), 76 | traj8(PoseSpline8(dt)), 77 | traj9(PoseSpline9(dt)) 78 | {}; 79 | 80 | void setStartTime(double t) 81 | { 82 | SPLINE_SET(setStartTime, t); 83 | }; 84 | 85 | void extendKnotsTo(double t, SE3d pose) 86 | { 87 | SPLINE_SET(extendKnotsTo, t, pose); 88 | } 89 | 90 | void knots_push_back(SE3d pose) 91 | { 92 | SPLINE_SET(knots_push_back, pose); 93 | } 94 | 95 | void genRandomTrajectory(int n) 96 | { 97 | SPLINE_SET(genRandomTrajectory, n); 98 | } 99 | 100 | int numKnots() 101 | { 102 | SPLINE_GET(numKnots); 103 | } 104 | 105 | int order() 106 | { 107 | return X; 108 | } 109 | 110 | MatrixXd blendingMatrix(bool cummulative = false) 111 | { 112 | SPLINE_GET(blendingMatrix, cummulative); 113 | } 114 | 115 | double getDt() 116 | { 117 | SPLINE_GET(getDt); 118 | } 119 | 120 | void setKnot(SE3d pose, int i) 121 | { 122 | SPLINE_SET(setKnot, pose, i); 123 | } 124 | 125 | SE3d getKnot(int i) 126 | { 127 | SPLINE_GET(getKnot, i); 128 | } 129 | 130 | double getKnotTime(int i) 131 | { 132 | SPLINE_GET(getKnotTime, i); 133 | } 134 | 135 | SO3d &getKnotSO3(int i) 136 | { 137 | SPLINE_GET(getKnotSO3, i); 138 | } 139 | 140 | Vector3d &getKnotPos(int i) 141 | { 142 | SPLINE_GET(getKnotPos, i); 143 | } 144 | 145 | double minTime() 146 | { 147 | SPLINE_GET(minTime); 148 | } 149 | 150 | double maxTime() 151 | { 152 | SPLINE_GET(maxTime); 153 | } 154 | 155 | SE3d pose(double t) 156 | { 157 | SPLINE_GET(pose, t); 158 | } 159 | 160 | Vector3d rotVelBody(double t) 161 | { 162 | SPLINE_GET(rotVelBody, t); 163 | } 164 | 165 | Vector3d transVelWorld(double t) 166 | { 167 | SPLINE_GET(transVelWorld, t); 168 | } 169 | 170 | Vector3d transAccelWorld(double t) 171 | { 172 | SPLINE_GET(transAccelWorld, t); 173 | } 174 | 175 | pair computeTIndex(double t) 176 | { 177 | SPLINE_GET(computeTIndex, t); 178 | } 179 | 180 | bool TimeIsValid(double time, double tolerance = 0) 181 | { 182 | return (minTime() + tolerance < time && time < maxTime() - tolerance); 183 | } 184 | }; 185 | 186 | /* #endregion "Dynamic order" spline --------------------------------------------------------------------------------*/ -------------------------------------------------------------------------------- /include/basalt/spline/spline_segment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace basalt 9 | { 10 | 11 | // Define time types 12 | using time_span_t = std::pair; 13 | using time_init_t = std::initializer_list; 14 | 15 | struct MetaData 16 | { 17 | virtual size_t NumParameters() const = 0; 18 | }; 19 | 20 | template 21 | struct SplineSegmentMeta : public MetaData 22 | { 23 | static constexpr int N = _N; // Order of the spline. 24 | static constexpr int DEG = _N - 1; // Degree of the spline. 25 | 26 | double t0; // First valid time 27 | double dt; // Knot spacing 28 | size_t n; // Number of knots 29 | 30 | SplineSegmentMeta(double _t0, double _dt, size_t _n = 0) 31 | : t0(_t0), dt(_dt), n(_n) {} 32 | 33 | size_t NumParameters() const override 34 | { 35 | return n; 36 | } 37 | 38 | double MinTime() const 39 | { 40 | return t0; 41 | } 42 | 43 | double MaxTime() const 44 | { 45 | return t0 + (n - DEG) * dt; 46 | } 47 | 48 | template 49 | size_t PotentiallyUnsafeFloor(T x) const 50 | { 51 | return static_cast(std::floor(x)); 52 | } 53 | 54 | // This way of treating Jets are potentially unsafe, hence the function name 55 | template 56 | size_t PotentiallyUnsafeFloor(const ceres::Jet &x) const 57 | { 58 | return static_cast(ceres::floor(x.a)); 59 | }; 60 | 61 | template 62 | bool computeTIndex(const T ×tamp, T &u, size_t &s) const 63 | { 64 | T t = timestamp; 65 | if (timestamp >= T(MaxTime())) 66 | t = timestamp - T(1e-9); // 1ns 67 | else if (timestamp < T(MinTime())) 68 | t = timestamp + T(1e-9); 69 | 70 | if (t >= T(MinTime()) && t < T(MaxTime())) 71 | { 72 | T st = (t - T(t0)) / T(dt); 73 | s = PotentiallyUnsafeFloor(st); // 取整数部分 74 | u = st - T(s); // 取小数部分 75 | return true; 76 | } 77 | else 78 | { 79 | return false; 80 | } 81 | } 82 | }; 83 | 84 | template 85 | struct SplineMeta 86 | { 87 | std::vector> segments; 88 | 89 | size_t NumParameters() const 90 | { 91 | size_t n = 0; 92 | for (auto &segment_meta : segments) 93 | { 94 | n += segment_meta.NumParameters(); 95 | } 96 | return n; 97 | } 98 | 99 | template 100 | bool ComputeSplineIndex(const T ×tamp, size_t &idx, T &u) const 101 | { 102 | idx = 0; 103 | for (auto const &seg : segments) 104 | { 105 | size_t s = 0; 106 | if (seg.computeTIndex(timestamp, u, s)) 107 | { 108 | idx += s; 109 | return true; 110 | } 111 | else 112 | { 113 | idx += seg.NumParameters(); 114 | } 115 | } 116 | std::cout << std::fixed << std::setprecision(15) << "ComputeSplineIndex Problem :" << timestamp << " : " 117 | << segments[0].t0 << " : " << segments[0].MaxTime() << std::endl; 118 | return false; 119 | } 120 | }; 121 | 122 | } // namespace basalt 123 | -------------------------------------------------------------------------------- /include/basalt/utils/assert.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | 36 | @file 37 | @brief Assertions used in the project 38 | */ 39 | 40 | #pragma once 41 | 42 | #include 43 | 44 | namespace basalt 45 | { 46 | 47 | #define UNUSED(x) (void)(x) 48 | 49 | inline void assertion_failed(char const *expr, char const *function, 50 | char const *file, long line) 51 | { 52 | std::cerr << "***** Assertion (" << expr << ") failed in " << function 53 | << ":\n" 54 | << file << ':' << line << ":" << std::endl; 55 | std::abort(); 56 | } 57 | 58 | inline void assertion_failed_msg(char const *expr, char const *msg, 59 | char const *function, char const *file, 60 | long line) 61 | { 62 | std::cerr << "***** Assertion (" << expr << ") failed in " << function 63 | << ":\n" 64 | << file << ':' << line << ": " << msg << std::endl; 65 | std::abort(); 66 | } 67 | } // namespace basalt 68 | 69 | #define BASALT_LIKELY(x) __builtin_expect(x, 1) 70 | 71 | #if defined(BASALT_DISABLE_ASSERTS) 72 | 73 | #define BASALT_ASSERT(expr) ((void)0) 74 | 75 | #define BASALT_ASSERT_MSG(expr, msg) ((void)0) 76 | 77 | #define BASALT_ASSERT_STREAM(expr, msg) ((void)0) 78 | 79 | #else 80 | 81 | #define BASALT_ASSERT(expr) \ 82 | (BASALT_LIKELY(!!(expr)) \ 83 | ? ((void)0) \ 84 | : ::basalt::assertion_failed(#expr, __PRETTY_FUNCTION__, __FILE__, \ 85 | __LINE__)) 86 | 87 | #define BASALT_ASSERT_MSG(expr, msg) \ 88 | (BASALT_LIKELY(!!(expr)) \ 89 | ? ((void)0) \ 90 | : ::basalt::assertion_failed_msg(#expr, msg, __PRETTY_FUNCTION__, \ 91 | __FILE__, __LINE__)) 92 | 93 | #define BASALT_ASSERT_STREAM(expr, msg) \ 94 | (BASALT_LIKELY(!!(expr)) \ 95 | ? ((void)0) \ 96 | : (std::cerr << msg << std::endl, \ 97 | ::basalt::assertion_failed(#expr, __PRETTY_FUNCTION__, __FILE__, \ 98 | __LINE__))) 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /include/basalt/utils/eigen_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt-headers.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | 35 | @file 36 | @brief Definition of the standard containers with Eigen allocator. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | namespace Eigen 49 | { 50 | 51 | template 52 | using aligned_vector = std::vector>; 53 | 54 | template 55 | using aligned_deque = std::deque>; 56 | 57 | template 58 | using aligned_map = std::map, 59 | Eigen::aligned_allocator>>; 60 | 61 | template 62 | using aligned_unordered_map = 63 | std::unordered_map, std::equal_to, 64 | Eigen::aligned_allocator>>; 65 | 66 | inline Eigen::Affine3d getTransBetween(Eigen::Vector3d trans_start, 67 | Eigen::Quaterniond rot_start, 68 | Eigen::Vector3d trans_end, 69 | Eigen::Quaterniond rot_end) 70 | { 71 | Eigen::Translation3d t_s(trans_start(0), trans_start(1), trans_start(2)); 72 | Eigen::Translation3d t_e(trans_end(0), trans_end(1), trans_end(2)); 73 | 74 | Eigen::Affine3d start = t_s * rot_start.toRotationMatrix(); 75 | Eigen::Affine3d end = t_e * rot_end.toRotationMatrix(); 76 | 77 | Eigen::Affine3d result = start.inverse() * end; 78 | return result; 79 | } 80 | 81 | } // namespace Eigen 82 | -------------------------------------------------------------------------------- /include/factor/PointToPlaneDisFactor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of VIRALC. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * School of EEE 6 | * Nanyang Technological Univertsity, Singapore 7 | * 8 | * For more information please see . 9 | * or . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * VIRALC is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * VIRALC is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with VIRALC. If not, see . 25 | */ 26 | 27 | // 28 | // Created by Thien-Minh Nguyen on 15/12/20. 29 | // 30 | 31 | #include 32 | #include 33 | 34 | #include "../utility.h" 35 | 36 | class Point2PlaneDisFactor : public ceres::SizedCostFunction<1, 7> 37 | { 38 | public: 39 | Point2PlaneDisFactor() = delete; 40 | Point2PlaneDisFactor(const Eigen::Vector3d &point, 41 | const Eigen::Vector4d &coeff) : point_(point), sqrt_info_static_(1.0) 42 | { 43 | normal_ << coeff.x(), coeff.y(), coeff.z(); 44 | offset_ = coeff.w(); 45 | } 46 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 47 | { 48 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 49 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 50 | 51 | double residual = normal_.transpose() * (Qi * point_ + Pi) + offset_; 52 | 53 | residuals[0] = sqrt_info_static_*residual; 54 | 55 | if(jacobians) 56 | { 57 | Eigen::Map> jacobian_pose(jacobians[0]); 58 | jacobian_pose.setZero(); 59 | jacobian_pose.block<1, 3>(0, 0) = normal_.transpose(); 60 | jacobian_pose.block<1, 3>(0, 3) = -normal_.transpose() 61 | *Qi.toRotationMatrix() 62 | *Util::skewSymmetric(point_); 63 | jacobian_pose = sqrt_info_static_*jacobian_pose; 64 | } 65 | 66 | return true; 67 | } 68 | // void Check(double **parameters); 69 | 70 | private: 71 | Eigen::Vector3d point_; 72 | Eigen::Vector3d normal_; 73 | double offset_; 74 | 75 | double sqrt_info_static_; 76 | }; -------------------------------------------------------------------------------- /include/factor/RelOdomFactor.h: -------------------------------------------------------------------------------- 1 | #ifndef slict_RELODOMFACTOR_H_ 2 | #define slict_RELODOMFACTOR_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "../utility.h" 8 | 9 | class RelOdomFactor : public ceres::SizedCostFunction<6, 7, 7> 10 | { 11 | private: 12 | 13 | Eigen::Vector3d p_odom_i; 14 | Eigen::Vector3d p_odom_j; 15 | 16 | Eigen::Quaterniond q_odom_i; 17 | Eigen::Quaterniond q_odom_j; 18 | 19 | double p_odom_n = 0.25; 20 | double q_odom_n = 0.25; 21 | 22 | Eigen::Matrix sqrt_info; 23 | 24 | public: 25 | 26 | RelOdomFactor() = delete; 27 | RelOdomFactor( 28 | 29 | const Eigen::Vector3d &_p_odom_i, 30 | const Eigen::Vector3d &_p_odom_j, 31 | 32 | const Eigen::Quaterniond &_q_odom_i, 33 | const Eigen::Quaterniond &_q_odom_j, 34 | 35 | const double _p_odom_n, 36 | const double _q_odom_n) 37 | : p_odom_i(_p_odom_i), p_odom_j(_p_odom_j), 38 | q_odom_i(_q_odom_i), q_odom_j(_q_odom_j), 39 | p_odom_n(_p_odom_n), q_odom_n(_q_odom_n) 40 | { 41 | // Eigen::Matrix Roti = q_odom_i.toRotationMatrix(); 42 | // Eigen::Matrix Roti_inv = Roti.transpose(); 43 | // Eigen::Matrix DeltaRot = Roti_inv*(q_odom_j.toRotationMatrix()); 44 | 45 | double q_odom_n_sq = q_odom_n*q_odom_n; 46 | Eigen::Matrix Pphi; 47 | Pphi << q_odom_n_sq, 0, 0, 48 | 0, q_odom_n_sq, 0, 49 | 0, 0, q_odom_n_sq; 50 | 51 | double p_odom_n_sq = p_odom_n*p_odom_n; 52 | Eigen::Matrix Pnu; 53 | Pnu << p_odom_n_sq, 0, 0, 54 | 0, p_odom_n_sq, 0, 55 | 0, 0, p_odom_n_sq; 56 | 57 | // Calculate the covariance matrix 58 | Eigen::Matrix P_vio = Eigen::Matrix::Zero(); 59 | P_vio.block<3, 3>(0, 0) = Pphi; 60 | P_vio.block<3, 3>(3, 3) = Pnu; 61 | 62 | // printf("Hello 3. P_vio.: row %d, col %d\n", P_vio.rows(), P_vio.cols()); 63 | // std::cout << P_vio << std::endl; 64 | 65 | sqrt_info = Eigen::LLT>(P_vio.inverse()).matrixL().transpose(); 66 | } 67 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 68 | { 69 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 70 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 71 | 72 | Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); 73 | Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]); 74 | 75 | Eigen::Map < Eigen::Matrix > residual(residuals); 76 | 77 | Eigen::Vector3d delta_p = q_odom_i.inverse()*(p_odom_j - p_odom_i); 78 | Eigen::Quaterniond delta_q = q_odom_i.inverse()*(q_odom_j); 79 | 80 | residual.block<3, 1>(0, 0) = Qi.inverse() * (Pj - Pi) - delta_p; 81 | residual.block<3, 1>(3, 0) = 2 * (delta_q.inverse() * (Qi.inverse() * Qj)).vec(); 82 | 83 | residual = sqrt_info * residual; 84 | 85 | if (jacobians) 86 | { 87 | if(jacobians[0]) 88 | { 89 | Eigen::Map> jacobian_pose_i(jacobians[0]); 90 | 91 | jacobian_pose_i.setZero(); 92 | 93 | jacobian_pose_i.block<3, 3>(0, 0) = -Qi.inverse().toRotationMatrix(); 94 | 95 | jacobian_pose_i.block<3, 3>(0, 3) = Util::skewSymmetric( Qi.inverse() * (Pj - Pi) ); 96 | 97 | jacobian_pose_i.block<3, 3>(3, 3) = -(Util::Qright(delta_q) * Util::Qleft(Qj.inverse() * Qi)).bottomRightCorner<3, 3>(); 98 | 99 | jacobian_pose_i = sqrt_info * jacobian_pose_i; 100 | 101 | if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) 102 | { 103 | printf("numerical unstable in vio factor jacobian.\n"); 104 | // std::cout << pre_integration->jacobian << std::endl; 105 | // ROS_BREAK(); 106 | } 107 | 108 | } 109 | 110 | if(jacobians[1]) 111 | { 112 | 113 | Eigen::Map> jacobian_pose_j(jacobians[1]); 114 | 115 | jacobian_pose_j.setZero(); 116 | 117 | jacobian_pose_j.block<3, 3>(0, 0) = Qi.inverse().toRotationMatrix(); 118 | 119 | // jacobian_pose_i.block<3, 3>(0, 3) = Util::skewSymmetric( Qi.inverse() * (Pj - Pi) ); 120 | 121 | jacobian_pose_j.block<3, 3>(3, 3) = Util::Qleft(delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>(); 122 | 123 | jacobian_pose_j = sqrt_info * jacobian_pose_j; 124 | 125 | if (jacobian_pose_j.maxCoeff() > 1e8 || jacobian_pose_j.minCoeff() < -1e8) 126 | { 127 | printf("numerical unstable in vio factor jacobian.\n"); 128 | // std::cout << pre_integration->jacobian << std::endl; 129 | // ROS_BREAK(); 130 | } 131 | 132 | } 133 | 134 | } 135 | 136 | return true; 137 | } 138 | }; 139 | 140 | #endif //slict_RELODOMFACTOR_H_ -------------------------------------------------------------------------------- /include/factor/VelocityAnalyticFactor.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "basalt/spline/ceres_spline_helper.h" 5 | #include "basalt/utils/sophus_utils.hpp" 6 | #include "utility.h" 7 | 8 | using SO3d = Sophus::SO3; 9 | using SE3d = Sophus::SE3; 10 | 11 | class VelocityAnalyticFactor : public ceres::CostFunction 12 | { 13 | public: 14 | 15 | VelocityAnalyticFactor(const Vector3d &vel_meas_, double wV_, int N_, double Dt_, double s_) 16 | : vel_meas (vel_meas_ ), 17 | wV (wV_ ), 18 | N (N_ ), 19 | Dt (Dt_ ), 20 | s (s_ ) 21 | { 22 | // 6-element residual: (3x1 omega, 3x1 a, 3x1 bw, 3x1 ba) 23 | set_num_residuals(3); 24 | 25 | for (size_t j = 0; j < N; j++) 26 | mutable_parameter_block_sizes()->push_back(3); 27 | 28 | // Calculate the spline-defining quantities: 29 | 30 | // Blending matrix for position, in standard form 31 | Matrix B = basalt::computeBlendingMatrix(N); 32 | 33 | // Inverse of knot length 34 | double Dt_inv = 1.0/Dt; 35 | 36 | // Time power derivative 37 | Matrix Udot = Matrix::Zero(N); 38 | for (int j = 1; j < N; j++) 39 | Udot(j) = j * std::pow(s, j - 1); 40 | 41 | // Lambda dot 42 | lambda_P_dot = Dt_inv * B * Udot; 43 | } 44 | 45 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 46 | { 47 | /* #region Map the memory to control points -----------------------------------------------------------------*/ 48 | 49 | // Indexing offsets for the states 50 | size_t P_offset = 0; 51 | 52 | // Map parameters to the control point states 53 | Vector3d Pos[N]; 54 | for (int j = 0; j < N; j++) 55 | Pos[j] = Eigen::Map(parameters[P_offset + j]); 56 | 57 | /* #endregion Map the memory to control points --------------------------------------------------------------*/ 58 | 59 | /* #region Calculate the residual ---------------------------------------------------------------------------*/ 60 | 61 | // The following use the formulations in the paper 2020 CVPR: 62 | // "Efficient derivative computation for cumulative b-splines on lie groups." 63 | // Sommer, Christiane, Vladyslav Usenko, David Schubert, Nikolaus Demmel, and Daniel Cremers. 64 | // Some errors in the paper is corrected 65 | 66 | // Predicted velocity 67 | Vector3d v_inW_Bt(0, 0, 0); 68 | for (int j = 0; j < N; j++) 69 | v_inW_Bt += lambda_P_dot[j]*Pos[j]; 70 | 71 | // Velocity residual 72 | Vector3d del = (v_inW_Bt - vel_meas); 73 | 74 | // Residual 75 | Eigen::Map> residual(residuals); 76 | residual = wV*del; 77 | 78 | /* #endregion Calculate the residual ------------------------------------------------------------------------*/ 79 | 80 | if (!jacobians) 81 | return true; 82 | 83 | size_t idx; 84 | for (size_t j = 0; j < N; j++) 85 | { 86 | idx = P_offset + j; 87 | if (jacobians[idx]) 88 | { 89 | Eigen::Map> J_knot_p(jacobians[idx]); 90 | J_knot_p.setZero(); 91 | 92 | double wV_lambda_P_dot = wV*lambda_P_dot[j]; 93 | 94 | /// for velocity residual 95 | J_knot_p.block<3, 3>(0, 0) = Vector3d(wV_lambda_P_dot, wV_lambda_P_dot, wV_lambda_P_dot).asDiagonal(); 96 | } 97 | } 98 | 99 | return true; 100 | } 101 | 102 | private: 103 | 104 | Vector3d vel_meas; 105 | 106 | double wV; 107 | 108 | int N; 109 | double Dt; // Knot length 110 | double s; // Normalized time (t - t_i)/Dt 111 | 112 | // Lambda dot 113 | Matrix lambda_P_dot; 114 | }; 115 | -------------------------------------------------------------------------------- /include/ikdTree/README.md: -------------------------------------------------------------------------------- 1 | # ikd-Tree 2 | ikd-Tree is an incremental k-d tree for robotic applications. 3 | -------------------------------------------------------------------------------- /launch/run_blocks2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | [56.14, 106.63, 7.58, 18, 0, 0] 20 | [55.07, 105.76, 7.71, -17, 0, 0] 21 | [55.65, 104.97, 7.68, -32, 0, 0] 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 58 | 59 | 60 | 61 | 62 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /launch/run_clins_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 34 | 35 | 36 | 37 | 38 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /launch/run_elfant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | [ 1.0, 0.0, 0.0, 15, 63 | 0.0, 1.0, 0.0, 68, 64 | 0.0, 0.0, 1.0, 0, 65 | 0.0, 0.0, 0.0, 1.00 ] 66 | 67 | 68 | 69 | 70 | 71 | 72 | [ 1.0, 0.0, 0.0, 0.082, 73 | 0.0, -1.0, 0.0, 0.052, 74 | 0.0, 0.0, -1.0, -0.026, 75 | 0.0, 0.0, 0.0, 1.000 ] 76 | 77 | 78 | 79 | 80 | 81 | 82 | [ 1.0, 0.0, 0.0, 0.082, 83 | 0.0, -1.0, 0.0, 0.052, 84 | 0.0, 0.0, -1.0, -0.026, 85 | 0.0, 0.0, 0.0, 1.000 ] 86 | 87 | 88 | 89 | 90 | 91 | 92 | 94 | 95 | 96 | 97 | 98 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /launch/run_elfant_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | [ 1.0, 0.0, 0.0, 15, 63 | 0.0, 1.0, 0.0, 68, 64 | 0.0, 0.0, 1.0, 0, 65 | 0.0, 0.0, 0.0, 1.0 ] 66 | 67 | 68 | 69 | 70 | 71 | 72 | [ 1.0, 0.0, 0.0, 0.082, 73 | 0.0, -1.0, 0.0, 0.052, 74 | 0.0, 0.0, -1.0, -0.026, 75 | 0.0, 0.0, 0.0, 1.000 ] 76 | 77 | 78 | 79 | 80 | 81 | 82 | [ 1.0, 0.0, 0.0, 0.082, 83 | 0.0, -1.0, 0.0, 0.052, 84 | 0.0, 0.0, -1.0, -0.026, 85 | 0.0, 0.0, 0.0, 1.000 ] 86 | 87 | 88 | 89 | 90 | 91 | 92 | 94 | 95 | 96 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /launch/run_fuPo_handheld.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 42 | 43 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/run_helmet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | [56.14, 106.63, 7.58, 18, 0, 0] 24 | [55.07, 105.76, 7.71, -17, 0, 0] 25 | [55.65, 104.97, 7.68, -32, 0, 0] 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /launch/run_hilti2022.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | 47 | 48 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /launch/run_hilti2023.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 56 | 57 | 58 | 59 | 60 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/run_jacobian_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /launch/run_kthforest.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /launch/run_liosam_data.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /launch/run_m2dgr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 35 | 36 | 37 | 38 | 39 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /launch/run_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 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /launch/run_ntuviral.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | 56 | 57 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /launch/run_oxford.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 57 | 58 | 59 | 60 | 61 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /launch/run_stillwh.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | [ 76.67, 69.93, 1.55, -84.86, 1.5, 0.00] 34 | [ 76.30, 70.00, 1.55, -83.72, 1.8, 0.00] 35 | [ 76.33, 69.91, 1.55, -85.00, 1.8, 0.00] 36 | [ 76.51, 69.98, 1.59, -84.30, 2.0, -0.58] 37 | [ 76.43, 69.98, 1.50, -84.89, 2.0, -0.84] 38 | [ 76.43, 69.98, 1.55, -84.89, 2.0, -0.84] 39 | 40 | [ 76.85, 69.84, 1.55, -83.60, 2.0, -0.46] 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 69 | 70 | 71 | 72 | 73 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /launch/run_urbanloco.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | 62 | 63 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /launch/run_whu.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 43 | 44 | 45 | 46 | 47 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /msg/FeatureCloud.msg: -------------------------------------------------------------------------------- 1 | # This file is part of slict. 2 | # 3 | # Copyright (C) 2020 Thien-Minh Nguyen , 4 | # School of EEE 5 | # Nanyang Technological Univertsity, Singapore 6 | # 7 | # For more information please see . 8 | # or . 9 | # If you use this code, please cite the respective publications as 10 | # listed on the above websites. 11 | # 12 | # slict is free software: you can redistribute it and/or modify 13 | # it under the terms of the GNU General Public License as published by 14 | # the Free Software Foundation, either version 3 of the License, or 15 | # (at your option) any later version. 16 | # 17 | # slict is distributed in the hope that it will be useful, 18 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | # GNU General Public License for more details. 21 | # 22 | # You should have received a copy of the GNU General Public License 23 | # along with slict. If not, see . 24 | 25 | # 26 | # Created by Thien-Minh Nguyen on 15/12/20. 27 | # 28 | 29 | # Cloud Info 30 | Header header 31 | 32 | int32[] startRingIndex 33 | int32[] endRingIndex 34 | 35 | int32[] pointColInd # point column index in range image 36 | float32[] pointRange # point range 37 | 38 | float64 scanStartTime 39 | float64 scanEndTime 40 | 41 | # To inform the pose of the pointcloud 42 | geometry_msgs/Pose pose 43 | 44 | # Point cloud messages 45 | sensor_msgs/PointCloud2 extracted_cloud # extracted cloud full 46 | sensor_msgs/PointCloud2 edge_cloud # extracted corner feature 47 | sensor_msgs/PointCloud2 surf_cloud # extracted surface feature 48 | 49 | # Imu messges from start time to end time 50 | sensor_msgs/Imu[] imu_msgs -------------------------------------------------------------------------------- /msg/TimeLog.msg: -------------------------------------------------------------------------------- 1 | # This file is part of slict. 2 | # 3 | # Copyright (C) 2020 Thien-Minh Nguyen , 4 | # School of EEE 5 | # Nanyang Technological Univertsity, Singapore 6 | # 7 | # For more information please see . 8 | # or . 9 | # If you use this code, please cite the respective publications as 10 | # listed on the above websites. 11 | # 12 | # slict is free software: you can redistribute it and/or modify 13 | # it under the terms of the GNU General Public License as published by 14 | # the Free Software Foundation, either version 3 of the License, or 15 | # (at your option) any later version. 16 | # 17 | # slict is distributed in the hope that it will be useful, 18 | # but WITHOUT ANY WARRANTY without even the implied warranty of 19 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | # GNU General Public License for more details. 21 | # 22 | # You should have received a copy of the GNU General Public License 23 | # along with slict. If not, see . 24 | 25 | # 26 | # Created by Thien-Minh Nguyen on 15/12/20. 27 | # 28 | 29 | # Cloud Info 30 | Header header # Header time stamp is the same with the latest feature batch stamp 31 | 32 | int32 OptNum # Optimization counter 33 | 34 | int32 ceres_iter # For ceres solver, indicating the number of iterations 35 | 36 | float64 t_insert 37 | float64[] t_prop 38 | float64[] t_desk 39 | float64[] t_assoc 40 | float64[] t_feasel 41 | float64[] t_prep 42 | float64[] t_hprior 43 | float64[] t_himu 44 | float64[] t_hlidar 45 | float64[] t_compute 46 | float64[] t_update 47 | float64[] t_marg 48 | float64[] t_solve 49 | float64[] t_aftslv 50 | float64 t_loop 51 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | slict 5 | 0.0.0 6 | The slict package 7 | 8 | tmn 9 | 10 | TODO 11 | 12 | catkin 13 | 14 | roscpp 15 | roscpp 16 | rospy 17 | rospy 18 | 19 | tf 20 | tf 21 | 22 | cv_bridge 23 | cv_bridge 24 | 25 | pcl_conversions 26 | pcl_conversions 27 | 28 | 30 | 31 | std_msgs 32 | std_msgs 33 | sensor_msgs 34 | sensor_msgs 35 | geometry_msgs 36 | geometry_msgs 37 | nav_msgs 38 | nav_msgs 39 | 40 | message_generation 41 | message_generation 42 | message_runtime 43 | message_runtime 44 | 45 | 46 | ufomap_msgs 47 | ufomap_msgs 48 | ufomap_ros 49 | ufomap_ros 50 | 51 | ufomap 52 | 53 | 54 | -------------------------------------------------------------------------------- /scripts/run_helmet.sh: -------------------------------------------------------------------------------- 1 | #!bin/bash 2 | 3 | ROS_PKG=slict 4 | 5 | catkin build $ROS_PKG 6 | source /home/$USER/dev_ws/devel/setup.bash 7 | 8 | # Get the current directory 9 | CURR_DIR=$(pwd) 10 | echo CURRENT DIR: $CURR_DIR 11 | 12 | # Get the location of the package 13 | PACKAGE_DIR=$(rospack find $ROS_PKG) 14 | echo $ROS_PKG DIR: $PACKAGE_DIR 15 | 16 | # Enable screen capture 17 | CAPTURE_SCREEN=false; 18 | # Enable data logging 19 | LOG_DATA=true; 20 | 21 | export DATASET_LOCATION=/media/$USER/mySataSSD1/DATASETS/Helmet 22 | export LAUNCH_FILE=run_helmet.launch 23 | 24 | # Find the available sequences 25 | SEQUENCES=( 26 | $DATASET_LOCATION/helmet_01 27 | $DATASET_LOCATION/helmet_02 28 | $DATASET_LOCATION/helmet_03 29 | ) 30 | 31 | #region -------------------------------------------------------------------------------------------------------------# 32 | 33 | EPOC_DIR=/media/$USER/mySataSSD1/DATASETS/Helmet/Experiment/slict/slict_noloop 34 | 35 | for seq in ${SEQUENCES[@]}; 36 | do 37 | ( 38 | printf "\n" 39 | seq_basename="$(basename $seq)" 40 | printf "Sequence: $seq. Basename: $seq_basename\n" 41 | 42 | ./run_one_bag.sh _LAUNCH_FILE=$LAUNCH_FILE \ 43 | _ROS_PKG=$ROS_PKG \ 44 | _ROSBAG_PATH="$seq/*.bag" \ 45 | _CAPTURE_SCREEN=$CAPTURE_SCREEN \ 46 | _LOG_DATA=$LOG_DATA \ 47 | _LOG_PATH="$EPOC_DIR/result_$seq_basename" \ 48 | _LOOP_EN=0 \ 49 | # _IMU_TOPIC=/vn200/imu \ 50 | printf "\n" 51 | ) 52 | done 53 | 54 | #endregion ----------------------------------------------------------------------------------------------------------# -------------------------------------------------------------------------------- /scripts/run_mcdviral.sh: -------------------------------------------------------------------------------- 1 | #!bin/bash 2 | 3 | ROS_PKG=slict 4 | 5 | catkin build $ROS_PKG 6 | source $ROS_PKG/../../devel/setup.bash 7 | 8 | # Get the current directory 9 | CURR_DIR=$(pwd) 10 | echo CURRENT DIR: $CURR_DIR 11 | 12 | # Get the location of the package 13 | PACKAGE_DIR=$(rospack find $ROS_PKG) 14 | echo $ROS_PKG DIR: $PACKAGE_DIR 15 | 16 | # Enable screen capture 17 | CAPTURE_SCREEN=false; 18 | # Enable data logging 19 | LOG_DATA=true; 20 | 21 | export DATASET_LOCATION=/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PublishedSequencesUnzipped 22 | export LAUNCH_FILE=run_mcdviral.launch 23 | 24 | # Find the available sequences 25 | SEQUENCES=( 26 | # $DATASET_LOCATION/kth_day_06 27 | # $DATASET_LOCATION/kth_day_09 28 | # $DATASET_LOCATION/kth_day_10 29 | # $DATASET_LOCATION/kth_night_01 30 | # $DATASET_LOCATION/kth_night_04 31 | # $DATASET_LOCATION/kth_night_05 32 | # $DATASET_LOCATION/ntu_day_01 33 | # $DATASET_LOCATION/ntu_day_02 34 | # $DATASET_LOCATION/ntu_day_10 35 | # $DATASET_LOCATION/ntu_night_04 36 | $DATASET_LOCATION/ntu_night_08 37 | # $DATASET_LOCATION/ntu_night_13 38 | # $DATASET_LOCATION/tuhh_day_02 39 | # $DATASET_LOCATION/tuhh_day_03 40 | # $DATASET_LOCATION/tuhh_day_04 41 | # $DATASET_LOCATION/tuhh_night_07 42 | # $DATASET_LOCATION/tuhh_night_08 43 | # $DATASET_LOCATION/tuhh_night_09 44 | ) 45 | 46 | EPOC_DIR=/media/$USER/mySataSSD1/DATASETS/MCDVIRAL/Experiment/test_slict_crash 47 | 48 | for n in {1..5}; do 49 | 50 | EPOC_DIR_N=${EPOC_DIR}/try_$n 51 | 52 | for seq in ${SEQUENCES[@]}; 53 | do 54 | ( 55 | printf "\n" 56 | seq_basename="$(basename $seq)" 57 | printf "Sequence: $seq. Basename: $seq_basename\n" 58 | 59 | ./run_one_bag.sh _LAUNCH_FILE=$LAUNCH_FILE \ 60 | _ROS_PKG=$ROS_PKG \ 61 | _ROSBAG_PATH="$seq/*.bag" \ 62 | _CAPTURE_SCREEN=$CAPTURE_SCREEN \ 63 | _LOG_DATA=$LOG_DATA \ 64 | _LOG_PATH="$EPOC_DIR_N/result_$seq_basename" \ 65 | _LOOP_EN=1 \ 66 | # _IMU_TOPIC=/vn200/imu \ 67 | 68 | printf "\n" 69 | ) 70 | done 71 | 72 | done -------------------------------------------------------------------------------- /scripts/run_mcdviral_slamprior.sh: -------------------------------------------------------------------------------- 1 | #!bin/bash 2 | 3 | ROS_PKG=slict 4 | 5 | catkin build $ROS_PKG 6 | source $ROS_PKG/../../devel/setup.bash 7 | 8 | # Get the current directory 9 | CURR_DIR=$(pwd) 10 | echo CURRENT DIR: $CURR_DIR 11 | 12 | # Get the location of the package 13 | PACKAGE_DIR=$(rospack find $ROS_PKG) 14 | echo $ROS_PKG DIR: $PACKAGE_DIR 15 | 16 | # Enable screen capture 17 | CAPTURE_SCREEN=false; 18 | # Enable data logging 19 | LOG_DATA=true; 20 | 21 | export DATASET_PUBLISHED=/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/PublishedSequencesUnzipped 22 | export DATASET_UNPUBLISHED=/media/tmn/mySataSSD1/DATASETS/MCDVIRAL/UnpublishedSequences 23 | export LAUNCH_FILE=run_mcdviral_slamprior.launch 24 | 25 | # Find the available sequences 26 | SEQUENCES=( 27 | $DATASET_PUBLISHED/ntu_day_01 28 | $DATASET_PUBLISHED/ntu_day_02 29 | $DATASET_PUBLISHED/ntu_day_10 30 | $DATASET_PUBLISHED/ntu_night_04 31 | $DATASET_PUBLISHED/ntu_night_08 32 | $DATASET_PUBLISHED/ntu_night_13 33 | $DATASET_UNPUBLISHED/ntu_day_03 34 | $DATASET_UNPUBLISHED/ntu_day_04 35 | $DATASET_UNPUBLISHED/ntu_day_05 36 | $DATASET_UNPUBLISHED/ntu_day_06 37 | $DATASET_UNPUBLISHED/ntu_day_07 38 | $DATASET_UNPUBLISHED/ntu_day_08 39 | $DATASET_UNPUBLISHED/ntu_day_09 40 | $DATASET_UNPUBLISHED/ntu_night_01 41 | $DATASET_UNPUBLISHED/ntu_night_02 42 | $DATASET_UNPUBLISHED/ntu_night_03 43 | $DATASET_UNPUBLISHED/ntu_night_05 44 | $DATASET_UNPUBLISHED/ntu_night_06 45 | $DATASET_UNPUBLISHED/ntu_night_07 46 | $DATASET_UNPUBLISHED/ntu_night_09 47 | $DATASET_UNPUBLISHED/ntu_night_10 48 | $DATASET_UNPUBLISHED/ntu_night_11 49 | $DATASET_UNPUBLISHED/ntu_night_12 50 | ) 51 | 52 | EPOC_DIR=/media/$USER/mySataSSD1/DATASETS/MCDVIRAL/Experiment/slict_slamprior_2 53 | 54 | for n in {1..3}; do 55 | 56 | EPOC_DIR_N=${EPOC_DIR}/try_$n 57 | 58 | for seq in ${SEQUENCES[@]}; 59 | do 60 | ( 61 | printf "\n" 62 | seq_basename="$(basename $seq)" 63 | printf "Sequence: $seq. Basename: $seq_basename\n" 64 | 65 | ./run_one_bag.sh _LAUNCH_FILE=$LAUNCH_FILE \ 66 | _ROS_PKG=$ROS_PKG \ 67 | _ROSBAG_PATH="$seq/*.bag" \ 68 | _CAPTURE_SCREEN=$CAPTURE_SCREEN \ 69 | _LOG_DATA=$LOG_DATA \ 70 | _LOG_PATH="$EPOC_DIR_N/result_$seq_basename" \ 71 | _LOOP_EN=0 \ 72 | # _IMU_TOPIC=/vn200/imu \ 73 | 74 | printf "\n" 75 | ) 76 | done 77 | 78 | done -------------------------------------------------------------------------------- /scripts/run_ntuviral.sh: -------------------------------------------------------------------------------- 1 | #!bin/bash 2 | 3 | ROS_PKG=slict 4 | 5 | # catkin build $ROS_PKG 6 | # source /home/$USER/ros_ws/slict_ws/devel/setup.bash 7 | 8 | # Get the current directory 9 | CURR_DIR=$(pwd) 10 | echo CURRENT DIR: $CURR_DIR 11 | 12 | # Get the location of the package 13 | PACKAGE_DIR=$(rospack find $ROS_PKG) 14 | echo $ROS_PKG DIR: $PACKAGE_DIR 15 | 16 | CAPTURE_SCREEN=false; 17 | LOG_DATA=true; 18 | 19 | DATASET_LOCATION=/media/$USER/mySataSSD21/DATASETS/NTU_VIRAL/DATA/ 20 | LAUNCH_FILE=run_ntuviral.launch 21 | 22 | # Find the available sequences 23 | SEQUENCES=( 24 | $DATASET_LOCATION/spms_01 25 | $DATASET_LOCATION/spms_02 26 | $DATASET_LOCATION/spms_03 27 | $DATASET_LOCATION/eee_01 28 | $DATASET_LOCATION/eee_02 29 | $DATASET_LOCATION/eee_03 30 | $DATASET_LOCATION/nya_01 31 | $DATASET_LOCATION/nya_02 32 | $DATASET_LOCATION/nya_03 33 | $DATASET_LOCATION/rtp_01 34 | $DATASET_LOCATION/rtp_02 35 | $DATASET_LOCATION/rtp_03 36 | $DATASET_LOCATION/tnp_01 37 | $DATASET_LOCATION/tnp_02 38 | $DATASET_LOCATION/tnp_03 39 | $DATASET_LOCATION/sbs_01 40 | $DATASET_LOCATION/sbs_02 41 | $DATASET_LOCATION/sbs_03 42 | ) 43 | 44 | EPOC_DIR=/media/$USER/mySataSSD21/DATASETS/NTU_VIRAL/Experiment/slict_30012024 45 | 46 | for n in {1..1}; do 47 | 48 | EPOC_DIR_N=${EPOC_DIR}/try_$n 49 | 50 | for seq in ${SEQUENCES[@]}; 51 | do 52 | ( 53 | printf "\n" 54 | seq_basename="$(basename $seq)" 55 | printf "Sequence: $seq. Basename: $seq_basename\n" 56 | 57 | ./run_one_bag.sh _LAUNCH_FILE=$LAUNCH_FILE \ 58 | _ROS_PKG=$ROS_PKG \ 59 | _ROSBAG_PATH="$seq/${seq_basename}_mod.bag" \ 60 | _CAPTURE_SCREEN=$CAPTURE_SCREEN \ 61 | _LOG_DATA=$LOG_DATA \ 62 | _LOG_PATH="$EPOC_DIR_N/result_$seq_basename" \ 63 | _LOOP_EN=0 \ 64 | # _IMU_TOPIC=/vn200/imu \ 65 | 66 | printf "\n" 67 | ) 68 | done 69 | 70 | done -------------------------------------------------------------------------------- /scripts/run_one_bag.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | for ARGUMENT in "$@" 3 | do 4 | KEY=$(echo $ARGUMENT | cut -f1 -d=) 5 | 6 | KEY_LENGTH=${#KEY} 7 | VALUE="${ARGUMENT:$KEY_LENGTH+1}" 8 | 9 | export "$KEY"="$VALUE" 10 | 11 | printf "$KEY: \t%s\n" "$VALUE" 12 | done 13 | 14 | 15 | # if [ -z "${_IMU_TOPIC+x}" ]; 16 | # then 17 | # _IMU_TOPIC=/imu/imu 18 | # echo echo "_IMU_TOPIC is not set. Defaulted to '$_IMU_TOPIC'"; 19 | # # else 20 | # # echo "_IMU_TOPIC is set: '$_IMU_TOPIC'"; 21 | # fi 22 | 23 | # if [ -z "${_CONFIG_SUITE+x}" ]; 24 | # then 25 | # _CONFIG_SUITE='' 26 | # echo echo "_CONFIG_SUITE is not set. Defaulted to '$_CONFIG_SUITE'"; 27 | # # else 28 | # # echo "_CONFIG_SUITE is set: '$_CONFIG_SUITE'"; 29 | # fi 30 | 31 | # Reassigning the arguments 32 | export LAUNCH_FILE=$_LAUNCH_FILE; 33 | export ROS_PKG=$_ROS_PKG; 34 | export ROSBAG_PATH=$_ROSBAG_PATH; 35 | export CAPTURE_SCREEN=$_CAPTURE_SCREEN; 36 | export LOG_DATA=$_LOG_DATA; 37 | export LOG_PATH=$_LOG_PATH; 38 | export LOOP_EN=$_LOOP_EN; 39 | # export IMU_TOPIC=$_IMU_TOPIC 40 | # export CONFIG_SUITE=$_CONFIG_SUITE 41 | 42 | # Begin the processing 43 | 44 | # Find the path to the package 45 | ROS_PKG_DIR=$(rospack find $ROS_PKG) 46 | 47 | # Notify the bag file 48 | echo BAG FILE: "${ROSBAG_PATH}"; 49 | 50 | if $LOG_DATA 51 | then 52 | 53 | # Create the log director 54 | mkdir -p $LOG_PATH/ ; 55 | # Copy the config folders for later references 56 | cp -R $ROS_PKG_DIR/config $LOG_PATH; 57 | cp -R $ROS_PKG_DIR/launch $LOG_PATH; 58 | cp -R $ROS_PKG_DIR/script $LOG_PATH; 59 | # Create folder for BA output 60 | mkdir -p $LOG_PATH/ba; 61 | 62 | fi 63 | 64 | #Notify the log file 65 | echo LOG DIR: $LOG_PATH; 66 | 67 | 68 | # Turn on the screen capture if selected 69 | if $CAPTURE_SCREEN 70 | then 71 | 72 | echo CAPTURING SCREEN ON; 73 | ( 74 | ffmpeg -video_size 1920x1080 -framerate 0.5 -f x11grab -i $DISPLAY+0,0 \ 75 | -loglevel quiet -y $LOG_PATH/screen.mp4 76 | ) & 77 | FFMPEG_PID=$! 78 | 79 | else 80 | 81 | echo CAPTURING SCREEN OFF; 82 | sleep 1; 83 | 84 | fi 85 | 86 | echo FFMPEG PID $FFMPEG_PID 87 | 88 | if $LOG_DATA 89 | then 90 | 91 | echo LOGGING ON; 92 | 93 | # Start the process 94 | ( 95 | # xterm -T "GTGEN ${AFFIX}" -geometry 138x40+1075+-8 -e \ 96 | # " 97 | roslaunch $ROS_PKG $LAUNCH_FILE \ 98 | autorun:=1 \ 99 | loop_en:=$LOOP_EN \ 100 | bag_file:=$ROSBAG_PATH \ 101 | exp_log_dir:=$LOG_PATH/ba \ 102 | # 2>&1 | tee -a $LOG_PATH/terminal_log.txt 103 | # " 104 | )& 105 | 106 | MAIN_PID=$! 107 | 108 | # Log the topics 109 | ( sleep 1; rostopic echo -p --nostr --noarr /pred_odom \ 110 | > $LOG_PATH/predict_odom.csv ) \ 111 | & \ 112 | ( sleep 1; rostopic echo -p --nostr --noarr /opt_odom \ 113 | > $LOG_PATH/opt_odom.csv ) \ 114 | & \ 115 | # ( sleep 1; rostopic echo -b $ROSBAG_PATH -p --nostr --noarr /leica/pose/relative \ 116 | # > $LOG_PATH/leica_pose.csv ) \ 117 | # & \ 118 | # ( sleep 1; rostopic echo -b $ROSBAG_PATH -p --nostr --noarr /dji_sdk/imu \ 119 | # > $LOG_PATH/dji_sdk_imu.csv ) \ 120 | # & \ 121 | # ( sleep 1; rostopic echo -b $ROSBAG_PATH -p --nostr --noarr $IMU_TOPIC \ 122 | # > $LOG_PATH/vn100_imu.csv ) \ 123 | # & \ 124 | ( sleep 1; rostopic echo -p --nostr --noarr /opt_stat \ 125 | > $LOG_PATH/opt_stat.csv ) \ 126 | & \ 127 | 128 | else 129 | 130 | echo LOGGING OFF; 131 | sleep 1; 132 | 133 | fi 134 | 135 | wait $MAIN_PID; 136 | 137 | # Close the screen recorder 138 | kill $FFMPEG_PID; 139 | 140 | exit; -------------------------------------------------------------------------------- /scripts/run_oxford.sh: -------------------------------------------------------------------------------- 1 | #!bin/bash 2 | 3 | ROS_PKG=slict 4 | 5 | catkin build $ROS_PKG 6 | # source /home/$USER/dev_ws/devel/setup.bash 7 | 8 | # Get the current directory 9 | CURR_DIR=$(pwd) 10 | echo CURRENT DIR: $CURR_DIR 11 | 12 | # Get the location of the package 13 | PACKAGE_DIR=$(rospack find $ROS_PKG) 14 | echo $ROS_PKG DIR: $PACKAGE_DIR 15 | 16 | # Enable screen capture 17 | CAPTURE_SCREEN=false; 18 | # Enable data logging 19 | LOG_DATA=true; 20 | 21 | LAUNCH_FILE=run_oxford.launch 22 | DATASET_LOCATION=/media/tmn/mySataSSD1/DATASETS/NewerCollegeDataset 23 | 24 | # Find the available sequences 25 | SEQUENCES=( 26 | $DATASET_LOCATION/06_dynamic_spinning 27 | $DATASET_LOCATION/01_short_experiment 28 | $DATASET_LOCATION/02_long_experiment 29 | $DATASET_LOCATION/05_quad_with_dynamics 30 | # $DATASET_LOCATION/07_parkland_mound 31 | ) 32 | 33 | EPOC_DIR=/media/tmn/mySataSSD1/DATASETS/NewerCollegeDataset/Experiment/slict/oxford_noloop_26112024 34 | 35 | for n in {1..5}; do 36 | 37 | EPOC_DIR_N=${EPOC_DIR}/try_$n 38 | 39 | for seq in ${SEQUENCES[@]}; 40 | do 41 | ( 42 | printf "\n" 43 | seq_basename="$(basename $seq)" 44 | printf "Sequence: $seq. Basename: $seq_basename\n" 45 | 46 | ./run_one_bag.sh _LAUNCH_FILE=$LAUNCH_FILE \ 47 | _ROS_PKG=$ROS_PKG \ 48 | _ROSBAG_PATH="$seq/*.bag" \ 49 | _CAPTURE_SCREEN=$CAPTURE_SCREEN \ 50 | _LOG_DATA=$LOG_DATA \ 51 | _LOG_PATH="$EPOC_DIR_N/result_$seq_basename" \ 52 | _LOOP_EN=0 \ 53 | # _IMU_TOPIC=/vn200/imu \ 54 | 55 | printf "\n" 56 | ) 57 | done 58 | 59 | done -------------------------------------------------------------------------------- /src/MapManager.hpp: -------------------------------------------------------------------------------- 1 | #include "utility.h" 2 | 3 | class MapManager 4 | { 5 | 6 | private: 7 | 8 | // Node handle 9 | ros::NodeHandlePtr nh_ptr; 10 | 11 | // Publishing the prior map for visualization 12 | CloudXYZITPtr pmSurfGlobal; 13 | CloudXYZITPtr pmEdgeGlobal; 14 | CloudXYZITPtr priorMap; 15 | ros::Publisher priorMapPub; 16 | 17 | using ufoSurfelMap = ufo::map::SurfelMap; 18 | ufoSurfelMap surfelMapSurf; 19 | ufoSurfelMap surfelMapEdge; 20 | 21 | public: 22 | 23 | ~MapManager(); 24 | MapManager(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 25 | { 26 | pmSurfGlobal = CloudXYZITPtr(new CloudXYZIT()); 27 | pmEdgeGlobal = CloudXYZITPtr(new CloudXYZIT()); 28 | priorMap = CloudXYZITPtr(new CloudXYZIT()); 29 | 30 | // Initializing priormap 31 | priorMapPub = nh_ptr->advertise("/priormap", 10); 32 | 33 | string prior_map_dir = ""; 34 | nh_ptr->param("/prior_map_dir", prior_map_dir, string("")); 35 | 36 | // Read the pose log of priormap 37 | string pmPose_ = prior_map_dir + "/kfpose6d.pcd"; 38 | CloudPosePtr pmPose(new CloudPose()); 39 | pcl::io::loadPCDFile(pmPose_, *pmPose); 40 | 41 | int PM_KF_COUNT = pmPose->size(); 42 | printf("Prior map path %s. Num scans: %d\n", pmPose_.c_str(), pmPose->size()); 43 | 44 | // Reading the surf feature from log 45 | deque pmSurf(PM_KF_COUNT); 46 | deque pmEdge(PM_KF_COUNT); 47 | #pragma omp parallel for num_threads(MAX_THREADS) 48 | for (int i = 0; i < PM_KF_COUNT; i++) 49 | { 50 | pmSurf[i] = CloudXYZITPtr(new CloudXYZIT()); 51 | string pmSurf_ = prior_map_dir + "/pointclouds/" + "KfSurfPcl_" + to_string(i) + ".pcd"; 52 | pcl::io::loadPCDFile(pmSurf_, *pmSurf[i]); 53 | 54 | pmEdge[i] = CloudXYZITPtr(new CloudXYZIT()); 55 | string pmEdge_ = prior_map_dir + "/pointclouds/" + "KfEdgePcl_" + to_string(i) + ".pcd"; 56 | pcl::io::loadPCDFile(pmEdge_, *pmEdge[i]); 57 | 58 | // printf("Reading scan %d.\n", i); 59 | } 60 | 61 | printf("Merging the scans.\n"); 62 | 63 | // Merge the scans 64 | for (int i = 0; i < PM_KF_COUNT; i++) 65 | { 66 | *pmSurfGlobal += *pmSurf[i]; 67 | *pmEdgeGlobal += *pmEdge[i]; 68 | } 69 | 70 | // Publish the prior map for visualization 71 | *priorMap = *pmSurfGlobal + *pmEdgeGlobal; 72 | { 73 | pcl::UniformSampling downsampler; 74 | downsampler.setRadiusSearch(0.1); 75 | downsampler.setInputCloud(priorMap); 76 | downsampler.filter(*priorMap); 77 | } 78 | 79 | Util::publishCloud(priorMapPub, *priorMap, ros::Time::now(), "world"); 80 | 81 | printf(KYEL "Surfelizing the scans.\n" RESET); 82 | 83 | double leaf_size = 0.1; // To-do: change this to param 84 | // Surfelize the surf map 85 | surfelMapSurf = ufoSurfelMap(leaf_size); 86 | insertCloudToSurfelMap(surfelMapSurf, *pmSurfGlobal); 87 | // Surfelize the edge map 88 | surfelMapEdge = ufoSurfelMap(leaf_size); 89 | insertCloudToSurfelMap(surfelMapEdge, *pmEdgeGlobal); 90 | 91 | printf(KGRN "Done. Surfmap: %d. Edgemap: %d\n" RESET, surfelMapSurf.size(), surfelMapEdge.size()); 92 | } 93 | }; 94 | -------------------------------------------------------------------------------- /src/PointToMapAssoc.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_TO_MAP_ASSOC_H 2 | #define POINT_TO_MAP_ASSOC_H 3 | 4 | #include 5 | #include 6 | 7 | // Utils 8 | #include "utility.h" 9 | 10 | // Association 11 | #include "PointToMapAssoc.h" 12 | 13 | // Shorthands for ufomap 14 | namespace ufopred = ufo::map::predicate; 15 | using ufoSurfelMap = ufo::map::SurfelMap; 16 | using ufoSurfelMapPtr = boost::shared_ptr; 17 | using ufoNode = ufo::map::NodeBV; 18 | using ufoSphere = ufo::geometry::Sphere; 19 | using ufoPoint3 = ufo::map::Point3; 20 | 21 | class PointToMapAssoc 22 | { 23 | public: 24 | 25 | // Constructor 26 | PointToMapAssoc(ros::NodeHandlePtr &nh_); 27 | 28 | // Destructor 29 | ~PointToMapAssoc(); 30 | 31 | //Associate function 32 | void AssociatePointWithMap(PointXYZIT &fRaw, Vector3d &finB, Vector3d &finW, ufoSurfelMap &Map, LidarCoef &coef); 33 | 34 | private: 35 | 36 | // Node handler 37 | ros::NodeHandlePtr nh; 38 | 39 | // Surfel params 40 | int surfel_map_depth; 41 | int surfel_min_point; 42 | int surfel_min_depth; 43 | int surfel_query_depth; 44 | double surfel_intsect_rad; 45 | double surfel_min_plnrty; 46 | double dis_to_surfel_max; 47 | double score_min; 48 | 49 | }; 50 | 51 | #endif -------------------------------------------------------------------------------- /src/lidar_converters/BPearlToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | // const int queueLength = 2000; 33 | 34 | using namespace std; 35 | using namespace Eigen; 36 | using namespace pcl; 37 | 38 | class BPearlToOuster 39 | { 40 | private: 41 | // Node handler 42 | ros::NodeHandlePtr nh_ptr; 43 | 44 | ros::Subscriber bpearlCloudSub; 45 | ros::Publisher ousterCloudPub; 46 | 47 | bool remove_human_body = true; 48 | 49 | public: 50 | // Destructor 51 | ~BPearlToOuster() {} 52 | 53 | BPearlToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 54 | { 55 | bpearlCloudSub = nh_ptr->subscribe("/rslidar_points", 50, &BPearlToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 56 | ousterCloudPub = nh_ptr->advertise("/os_cloud_node/points", 50); 57 | } 58 | 59 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 60 | { 61 | pcl::PointCloud laserCloudBPearl; 62 | pcl::fromROSMsg(*msgIn, laserCloudBPearl); 63 | 64 | int cloudsize = laserCloudBPearl.size(); 65 | 66 | CloudOuster laserCloudOuster; 67 | laserCloudOuster.points.resize(cloudsize); 68 | laserCloudOuster.is_dense = true; 69 | 70 | static double hsToOusterIntensity = 1500.0/255.0; 71 | 72 | #pragma omp parallel for num_threads(MAX_THREADS) 73 | // double max_intensity = -1; 74 | for (size_t i = 0; i < cloudsize; i++) 75 | { 76 | // printf("Scan start %f. Scan end: %f. PC Stamp: %f\n", 77 | // laserCloudBPearl.front().timestamp, laserCloudBPearl.back().timestamp, 78 | // msgIn->header.stamp.toSec()); 79 | 80 | auto &src = laserCloudBPearl.points[i]; 81 | auto &dst = laserCloudOuster.points[i]; 82 | dst.x = src.x; 83 | dst.y = src.y; 84 | dst.z = src.z; 85 | dst.intensity = src.intensity * hsToOusterIntensity; 86 | dst.ring = src.ring; 87 | dst.t = (int)((src.timestamp- laserCloudBPearl.points[0].timestamp) * 1e9f); 88 | dst.range = sqrt(src.x*src.x + src.y*src.y + src.z*src.z)*1000.0; 89 | 90 | // Remove points on the carrier 91 | if(remove_human_body) 92 | { 93 | double yaw = Util::wrapTo360(atan2(dst.y, dst.x)*180/M_PI); 94 | if (yaw > 38 && yaw < 142 && dst.range < 0.5) 95 | { 96 | dst.range = 0.0; 97 | dst.x = dst.y = dst.z = 0; 98 | } 99 | } 100 | 101 | // max_intensity = (dst.intensity > max_intensity)?dst.intensity:max_intensity; 102 | } 103 | 104 | Util::publishCloud(ousterCloudPub, laserCloudOuster, 105 | msgIn->header.stamp - ros::Duration(0.1), 106 | msgIn->header.frame_id); 107 | } 108 | }; 109 | 110 | int main(int argc, char **argv) 111 | { 112 | ros::init(argc, argv, "bpearl_to_ouster"); 113 | ros::NodeHandle nh("~"); 114 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 115 | 116 | ROS_INFO(KGRN "----> BPearl to Ouster started" RESET); 117 | 118 | BPearlToOuster B2O(nh_ptr); 119 | 120 | ros::MultiThreadedSpinner spinner(0); 121 | spinner.spin(); 122 | 123 | return 0; 124 | } -------------------------------------------------------------------------------- /src/lidar_converters/HesaiToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | // const int queueLength = 2000; 33 | 34 | using namespace std; 35 | using namespace Eigen; 36 | using namespace pcl; 37 | 38 | class HesaiToOuster 39 | { 40 | private: 41 | // Node handler 42 | ros::NodeHandlePtr nh_ptr; 43 | 44 | ros::Subscriber hesaiCloudSub; 45 | ros::Publisher ousterCloudPub; 46 | 47 | bool remove_human_body = true; 48 | 49 | public: 50 | // Destructor 51 | ~HesaiToOuster() {} 52 | 53 | HesaiToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 54 | { 55 | hesaiCloudSub = nh_ptr->subscribe("/hesai/pandar", 50, &HesaiToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 56 | ousterCloudPub = nh_ptr->advertise("/os_cloud_node/points", 50); 57 | } 58 | 59 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 60 | { 61 | pcl::PointCloud laserCloudHesai; 62 | pcl::fromROSMsg(*msgIn, laserCloudHesai); 63 | 64 | int cloudsize = laserCloudHesai.size(); 65 | 66 | CloudOuster laserCloudOuster; 67 | laserCloudOuster.points.resize(cloudsize); 68 | laserCloudOuster.is_dense = true; 69 | 70 | static double hsToOusterIntensity = 1500.0/255.0; 71 | 72 | #pragma omp parallel for num_threads(MAX_THREADS) 73 | // double max_intensity = -1; 74 | for (size_t i = 0; i < cloudsize; i++) 75 | { 76 | auto &src = laserCloudHesai.points[i]; 77 | auto &dst = laserCloudOuster.points[i]; 78 | dst.x = src.x; 79 | dst.y = src.y; 80 | dst.z = src.z; 81 | dst.intensity = src.intensity * hsToOusterIntensity; 82 | dst.ring = src.ring; 83 | dst.t = (int)((src.timestamp - msgIn->header.stamp.toSec()) * 1e9f); 84 | dst.range = sqrt(src.x*src.x + src.y*src.y + src.z*src.z)*1000.0; 85 | 86 | // Remove points on the carrier 87 | if(remove_human_body) 88 | { 89 | double yaw = Util::wrapTo360(atan2(dst.y, dst.x)*180/M_PI); 90 | if (yaw > 38 && yaw < 142 && dst.range < 0.5) 91 | { 92 | dst.range = 0.0; 93 | dst.x = dst.y = dst.z = 0; 94 | } 95 | } 96 | 97 | // max_intensity = (dst.intensity > max_intensity)?dst.intensity:max_intensity; 98 | } 99 | 100 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 101 | } 102 | }; 103 | 104 | int main(int argc, char **argv) 105 | { 106 | ros::init(argc, argv, "hesai_to_ouster"); 107 | ros::NodeHandle nh("~"); 108 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 109 | 110 | ROS_INFO(KGRN "----> hesai to Ouster started" RESET); 111 | 112 | HesaiToOuster H2O(nh_ptr); 113 | 114 | ros::MultiThreadedSpinner spinner(0); 115 | spinner.spin(); 116 | 117 | return 0; 118 | } -------------------------------------------------------------------------------- /src/lidar_converters/Livox2ToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | #include 33 | 34 | // const int queueLength = 2000; 35 | 36 | using namespace std; 37 | using namespace Eigen; 38 | using namespace pcl; 39 | 40 | class Livox2ToOuster 41 | { 42 | private: 43 | // Node handler 44 | ros::NodeHandlePtr nh_ptr; 45 | 46 | ros::Subscriber livoxCloudSub; 47 | ros::Publisher ousterCloudPub; 48 | 49 | double intensityConvCoef = -1; 50 | 51 | int NUM_CORE; 52 | 53 | // bool remove_human_body = true; 54 | 55 | public: 56 | // Destructor 57 | ~Livox2ToOuster() {} 58 | 59 | Livox2ToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 60 | { 61 | NUM_CORE = omp_get_max_threads(); 62 | 63 | // Coefficient to convert the intensity from livox to ouster 64 | nh_ptr->param("intensityConvCoef", intensityConvCoef, 1.0); 65 | 66 | livoxCloudSub = nh_ptr->subscribe("/livox/lidar", 50, &Livox2ToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 67 | ousterCloudPub = nh_ptr->advertise("/livox/lidar_ouster", 50); 68 | } 69 | 70 | void cloudHandler(const livox_ros_driver2::CustomMsg::ConstPtr &msgIn) 71 | { 72 | int cloudsize = msgIn->points.size(); 73 | 74 | CloudOuster laserCloudOuster; 75 | laserCloudOuster.points.resize(cloudsize); 76 | laserCloudOuster.is_dense = true; 77 | 78 | #pragma omp parallel for num_threads(NUM_CORE) 79 | for (size_t i = 0; i < cloudsize; i++) 80 | { 81 | auto &src = msgIn->points[i]; 82 | auto &dst = laserCloudOuster.points[i]; 83 | dst.x = src.x; 84 | dst.y = src.y; 85 | dst.z = src.z; 86 | dst.intensity = src.reflectivity * intensityConvCoef; 87 | dst.ring = src.line; 88 | dst.t = src.offset_time; 89 | dst.range = sqrt(src.x * src.x + src.y * src.y + src.z * src.z)*1000; 90 | } 91 | 92 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 93 | } 94 | }; 95 | 96 | int main(int argc, char **argv) 97 | { 98 | ros::init(argc, argv, "velodyne_to_ouster"); 99 | ros::NodeHandle nh("~"); 100 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 101 | 102 | ROS_INFO(KGRN "----> Velodyne to Ouster started" RESET); 103 | 104 | Livox2ToOuster C2P(nh_ptr); 105 | 106 | ros::MultiThreadedSpinner spinner(0); 107 | spinner.spin(); 108 | 109 | return 0; 110 | } -------------------------------------------------------------------------------- /src/lidar_converters/LivoxToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | #include 33 | 34 | // const int queueLength = 2000; 35 | 36 | using namespace std; 37 | using namespace Eigen; 38 | using namespace pcl; 39 | 40 | class LivoxToOuster 41 | { 42 | private: 43 | // Node handler 44 | ros::NodeHandlePtr nh_ptr; 45 | 46 | ros::Subscriber livoxCloudSub; 47 | ros::Publisher ousterCloudPub; 48 | 49 | double intensityConvCoef = -1; 50 | 51 | int NUM_CORE; 52 | 53 | // bool remove_human_body = true; 54 | 55 | public: 56 | // Destructor 57 | ~LivoxToOuster() {} 58 | 59 | LivoxToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 60 | { 61 | NUM_CORE = omp_get_max_threads(); 62 | 63 | // Coefficient to convert the intensity from livox to ouster 64 | nh_ptr->param("intensityConvCoef", intensityConvCoef, 1.0); 65 | 66 | livoxCloudSub = nh_ptr->subscribe("/livox/lidar", 50, &LivoxToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 67 | ousterCloudPub = nh_ptr->advertise("/livox/lidar_ouster", 50); 68 | } 69 | 70 | void cloudHandler(const livox_ros_driver::CustomMsg::ConstPtr &msgIn) 71 | { 72 | int cloudsize = msgIn->points.size(); 73 | 74 | CloudOuster laserCloudOuster; 75 | laserCloudOuster.points.resize(cloudsize); 76 | laserCloudOuster.is_dense = true; 77 | 78 | #pragma omp parallel for num_threads(NUM_CORE) 79 | for (size_t i = 0; i < cloudsize; i++) 80 | { 81 | auto &src = msgIn->points[i]; 82 | auto &dst = laserCloudOuster.points[i]; 83 | dst.x = src.x; 84 | dst.y = src.y; 85 | dst.z = src.z; 86 | dst.intensity = src.reflectivity * intensityConvCoef; 87 | dst.ring = src.line; 88 | dst.t = src.offset_time; 89 | dst.range = sqrt(src.x * src.x + src.y * src.y + src.z * src.z)*1000; 90 | } 91 | 92 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 93 | } 94 | }; 95 | 96 | int main(int argc, char **argv) 97 | { 98 | ros::init(argc, argv, "velodyne_to_ouster"); 99 | ros::NodeHandle nh("~"); 100 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 101 | 102 | ROS_INFO(KGRN "----> Velodyne to Ouster started" RESET); 103 | 104 | LivoxToOuster C2P(nh_ptr); 105 | 106 | ros::MultiThreadedSpinner spinner(0); 107 | spinner.spin(); 108 | 109 | return 0; 110 | } -------------------------------------------------------------------------------- /src/lidar_converters/M2DGRToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | // const int queueLength = 2000; 33 | 34 | using namespace std; 35 | using namespace Eigen; 36 | using namespace pcl; 37 | 38 | class M2DGRToOuster 39 | { 40 | private: 41 | // Node handler 42 | ros::NodeHandlePtr nh_ptr; 43 | 44 | ros::Subscriber velodyneCloudSub; 45 | ros::Publisher ousterCloudPub; 46 | 47 | int NUM_CORE; 48 | 49 | // bool remove_human_body = true; 50 | 51 | public: 52 | // Destructor 53 | ~M2DGRToOuster() {} 54 | 55 | M2DGRToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 56 | { 57 | NUM_CORE = omp_get_max_threads(); 58 | 59 | // int remove_human_body_; 60 | // nh_ptr->param("remove_human_body", remove_human_body_, 0); 61 | // remove_human_body = (remove_human_body_ == 0)?false:true; 62 | 63 | velodyneCloudSub = nh_ptr->subscribe("/velodyne_points", 50, &M2DGRToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 64 | ousterCloudPub = nh_ptr->advertise("/os_cloud_node/points", 50); 65 | } 66 | 67 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 68 | { 69 | CloudVelodyne laserCloudVelodyne; 70 | pcl::fromROSMsg(*msgIn, laserCloudVelodyne); 71 | 72 | int cloudsize = laserCloudVelodyne.size(); 73 | 74 | CloudOuster laserCloudOuster; 75 | laserCloudOuster.points.resize(cloudsize); 76 | laserCloudOuster.is_dense = true; 77 | 78 | static double hsToOusterIntensity = 1.0; 79 | 80 | #pragma omp parallel for num_threads(NUM_CORE) 81 | for (size_t i = 0; i < cloudsize; i++) 82 | { 83 | auto &src = laserCloudVelodyne.points[i]; 84 | auto &dst = laserCloudOuster.points[i]; 85 | dst.x = src.x; 86 | dst.y = src.y; 87 | dst.z = src.z; 88 | dst.intensity = src.intensity * hsToOusterIntensity; 89 | dst.ring = src.ring; 90 | dst.t = (src.time + 0.1) * 1e9f; 91 | dst.range = sqrt(src.x * src.x + src.y * src.y + src.z * src.z)*1000.0; 92 | } 93 | 94 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 95 | } 96 | }; 97 | 98 | int main(int argc, char **argv) 99 | { 100 | ros::init(argc, argv, "velodyne_to_ouster"); 101 | ros::NodeHandle nh("~"); 102 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 103 | 104 | ROS_INFO(KGRN "----> Velodyne to Ouster started" RESET); 105 | 106 | M2DGRToOuster C2P(nh_ptr); 107 | 108 | ros::MultiThreadedSpinner spinner(0); 109 | spinner.spin(); 110 | 111 | return 0; 112 | } -------------------------------------------------------------------------------- /src/lidar_converters/MulranToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | // const int queueLength = 2000; 33 | 34 | using namespace std; 35 | using namespace Eigen; 36 | using namespace pcl; 37 | 38 | struct PointMulran 39 | { 40 | PCL_ADD_POINT4D; 41 | float intensity; 42 | uint32_t t; 43 | int32_t ring; 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 45 | } EIGEN_ALIGN16; 46 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointMulran, 47 | (float, x, x) (float, y, y) (float, z, z) 48 | (float, intensity, intensity) 49 | (uint32_t, t, t) 50 | (int32_t, ring, ring) 51 | ) 52 | 53 | typedef pcl::PointCloud CloudMulran; 54 | typedef pcl::PointCloud::Ptr CloudMulranPtr; 55 | 56 | class MulranToOuster 57 | { 58 | private: 59 | // Node handler 60 | ros::NodeHandlePtr nh_ptr; 61 | 62 | ros::Subscriber mulranCloudSub; 63 | ros::Publisher ousterCloudPub; 64 | 65 | int NUM_CORE; 66 | 67 | // bool remove_human_body = true; 68 | 69 | public: 70 | // Destructor 71 | ~MulranToOuster() {} 72 | 73 | MulranToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 74 | { 75 | NUM_CORE = omp_get_max_threads(); 76 | 77 | mulranCloudSub = nh_ptr->subscribe("/Ouster_Points", 50, &MulranToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 78 | ousterCloudPub = nh_ptr->advertise("/os_cloud_node/points", 50); 79 | } 80 | 81 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 82 | { 83 | CloudMulran laserCloudMulran; 84 | pcl::fromROSMsg(*msgIn, laserCloudMulran); 85 | 86 | int cloudsize = laserCloudMulran.size(); 87 | 88 | CloudOuster laserCloudOuster; 89 | laserCloudOuster.points.resize(cloudsize); 90 | laserCloudOuster.is_dense = true; 91 | 92 | #pragma omp parallel for num_threads(NUM_CORE) 93 | for (size_t i = 0; i < cloudsize; i++) 94 | { 95 | auto &src = laserCloudMulran.points[i]; 96 | auto &dst = laserCloudOuster.points[i]; 97 | dst.x = src.x; 98 | dst.y = src.y; 99 | dst.z = src.z; 100 | dst.intensity = src.intensity; 101 | dst.ring = src.ring; 102 | dst.t = src.t; 103 | dst.range = sqrt(src.x * src.x + src.y * src.y + src.z * src.z)*1000.0; 104 | // printf("Point %d, Time: %d\n", i, src.t); 105 | } 106 | 107 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 108 | } 109 | }; 110 | 111 | int main(int argc, char **argv) 112 | { 113 | ros::init(argc, argv, "velodyne_to_ouster"); 114 | ros::NodeHandle nh("~"); 115 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 116 | 117 | ROS_INFO(KGRN "----> Velodyne to Ouster started" RESET); 118 | 119 | MulranToOuster C2P(nh_ptr); 120 | 121 | ros::MultiThreadedSpinner spinner(0); 122 | spinner.spin(); 123 | 124 | return 0; 125 | } -------------------------------------------------------------------------------- /src/lidar_converters/OusterToVelodyne.cpp: -------------------------------------------------------------------------------- 1 | 2 | /** 3 | * This file is part of SLICT. 4 | * 5 | * Copyright (C) 2020 Thien-Minh Nguyen , 6 | * Division of RPL, KTH Royal Institute of Technology 7 | * 8 | * For more information please see . 9 | * or . 10 | * If you use this code, please cite the respective publications as 11 | * listed on the above websites. 12 | * 13 | * SLICT is free software: you can redistribute it and/or modify 14 | * it under the terms of the GNU General Public License as published by 15 | * the Free Software Foundation, either version 3 of the License, or 16 | * (at your option) any later version. 17 | * 18 | * SLICT is distributed in the hope that it will be useful, 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | * GNU General Public License for more details. 22 | * 23 | * You should have received a copy of the GNU General Public License 24 | * along with SLICT. If not, see . 25 | */ 26 | 27 | // 28 | // Created by Thien-Minh Nguyen on 01/08/22. 29 | // 30 | 31 | #include "utility.h" 32 | 33 | // #include 34 | 35 | // const int queueLength = 2000; 36 | 37 | using namespace std; 38 | using namespace Eigen; 39 | using namespace pcl; 40 | 41 | class OusterToVelodyne 42 | { 43 | private: 44 | // Node handler 45 | ros::NodeHandlePtr nh_ptr; 46 | 47 | ros::Subscriber ousterCloudSub; 48 | ros::Publisher velodyneCloudPub; 49 | 50 | int NUM_CORE; 51 | 52 | // bool remove_human_body = true; 53 | 54 | public: 55 | // Destructor 56 | ~OusterToVelodyne() {} 57 | 58 | OusterToVelodyne(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 59 | { 60 | NUM_CORE = omp_get_max_threads(); 61 | 62 | ousterCloudSub = nh_ptr->subscribe("/os_cloud_node/points", 50, &OusterToVelodyne::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 63 | velodyneCloudPub = nh_ptr->advertise("/velodyne_points", 50); 64 | } 65 | 66 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 67 | { 68 | CloudOuster laserCloudOuster; 69 | pcl::fromROSMsg(*msgIn, laserCloudOuster); 70 | 71 | int cloudsize = laserCloudOuster.size(); 72 | 73 | CloudVelodyne laserCloudVelodyne; 74 | laserCloudVelodyne.points.resize(cloudsize); 75 | laserCloudVelodyne.is_dense = true; 76 | 77 | static double ousterToHsIntensity = 1.0; 78 | 79 | #pragma omp parallel for num_threads(NUM_CORE) 80 | for (size_t i = 0; i < cloudsize; i++) 81 | { 82 | auto &src = laserCloudOuster.points[i]; 83 | auto &dst = laserCloudVelodyne.points[i]; 84 | dst.x = src.x; 85 | dst.y = src.y; 86 | dst.z = src.z; 87 | dst.intensity = src.intensity * ousterToHsIntensity; 88 | dst.ring = src.ring; 89 | dst.time = src.t * 1e-9f; // Converting from nanoseconds to seconds 90 | } 91 | 92 | Util::publishCloud(velodyneCloudPub, laserCloudVelodyne, msgIn->header.stamp, "velodyne"); 93 | } 94 | }; 95 | 96 | int main(int argc, char **argv) 97 | { 98 | ros::init(argc, argv, "ouster_to_velodyne"); 99 | ros::NodeHandle nh("~"); 100 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 101 | 102 | ROS_INFO(KGRN "----> Ouster to Velodyne started" RESET); 103 | 104 | OusterToVelodyne O2V(nh_ptr); 105 | 106 | ros::MultiThreadedSpinner spinner(0); 107 | spinner.spin(); 108 | 109 | return 0; 110 | } 111 | -------------------------------------------------------------------------------- /src/lidar_converters/VelodyneToOuster.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * This file is part of slict. 3 | * 4 | * Copyright (C) 2020 Thien-Minh Nguyen , 5 | * Division of RPL, KTH Royal Institute of Technology 6 | * 7 | * For more information please see . 8 | * or . 9 | * If you use this code, please cite the respective publications as 10 | * listed on the above websites. 11 | * 12 | * slict is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, either version 3 of the License, or 15 | * (at your option) any later version. 16 | * 17 | * slict is distributed in the hope that it will be useful, 18 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | * GNU General Public License for more details. 21 | * 22 | * You should have received a copy of the GNU General Public License 23 | * along with slict. If not, see . 24 | */ 25 | 26 | // 27 | // Created by Thien-Minh Nguyen on 01/08/22. 28 | // 29 | 30 | #include "utility.h" 31 | 32 | // const int queueLength = 2000; 33 | 34 | using namespace std; 35 | using namespace Eigen; 36 | using namespace pcl; 37 | 38 | class VelodyneToOuster 39 | { 40 | private: 41 | // Node handler 42 | ros::NodeHandlePtr nh_ptr; 43 | 44 | ros::Subscriber velodyneCloudSub; 45 | ros::Publisher ousterCloudPub; 46 | 47 | int NUM_CORE; 48 | 49 | // bool remove_human_body = true; 50 | 51 | public: 52 | // Destructor 53 | ~VelodyneToOuster() {} 54 | 55 | VelodyneToOuster(ros::NodeHandlePtr &nh_ptr_) : nh_ptr(nh_ptr_) 56 | { 57 | NUM_CORE = omp_get_max_threads(); 58 | 59 | // int remove_human_body_; 60 | // nh_ptr->param("remove_human_body", remove_human_body_, 0); 61 | // remove_human_body = (remove_human_body_ == 0)?false:true; 62 | 63 | velodyneCloudSub = nh_ptr->subscribe("/velodyne_points", 50, &VelodyneToOuster::cloudHandler, this, ros::TransportHints().tcpNoDelay()); 64 | ousterCloudPub = nh_ptr->advertise("/os_cloud_node/points", 50); 65 | } 66 | 67 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &msgIn) 68 | { 69 | // Check if the pointcloud has time 70 | static int has_time = -1; 71 | if (has_time == -1) 72 | { 73 | for(auto &field : msgIn->fields) 74 | if (field.name == "time") 75 | { 76 | has_time = 1; 77 | break; 78 | } 79 | 80 | if (has_time == -1) 81 | has_time = 0; 82 | 83 | printf(KRED "Msg has no time field\n" RESET); 84 | } 85 | 86 | CloudVelodyne laserCloudVelodyne; 87 | pcl::fromROSMsg(*msgIn, laserCloudVelodyne); 88 | 89 | int cloudsize = laserCloudVelodyne.size(); 90 | 91 | CloudOuster laserCloudOuster; 92 | laserCloudOuster.points.resize(cloudsize); 93 | laserCloudOuster.is_dense = true; 94 | 95 | static double hsToOusterIntensity = 1.0; 96 | 97 | #pragma omp parallel for num_threads(NUM_CORE) 98 | for (size_t i = 0; i < cloudsize; i++) 99 | { 100 | auto &src = laserCloudVelodyne.points[i]; 101 | auto &dst = laserCloudOuster.points[i]; 102 | dst.x = src.x; 103 | dst.y = src.y; 104 | dst.z = src.z; 105 | dst.intensity = src.intensity * hsToOusterIntensity; 106 | dst.ring = src.ring; 107 | dst.t = src.time * 1e9f; 108 | dst.range = sqrt(src.x * src.x + src.y * src.y + src.z * src.z)*1000; 109 | } 110 | 111 | Util::publishCloud(ousterCloudPub, laserCloudOuster, msgIn->header.stamp, msgIn->header.frame_id); 112 | } 113 | }; 114 | 115 | int main(int argc, char **argv) 116 | { 117 | ros::init(argc, argv, "velodyne_to_ouster"); 118 | ros::NodeHandle nh("~"); 119 | ros::NodeHandlePtr nh_ptr = boost::make_shared(nh); 120 | 121 | ROS_INFO(KGRN "----> Velodyne to Ouster started" RESET); 122 | 123 | VelodyneToOuster C2P(nh_ptr); 124 | 125 | ros::MultiThreadedSpinner spinner(0); 126 | spinner.spin(); 127 | 128 | return 0; 129 | } -------------------------------------------------------------------------------- /src/tmnSolver.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | // Basalt 6 | #include "basalt/spline/se3_spline.h" 7 | #include "basalt/spline/ceres_spline_helper.h" 8 | #include "basalt/spline/ceres_local_param.hpp" 9 | #include "basalt/spline/posesplinex.h" 10 | 11 | // Solver report 12 | #include "slict/OptStat.h" 13 | #include "slict/TimeLog.h" 14 | 15 | // Utils 16 | #include "utility.h" 17 | 18 | // Association 19 | #include "PointToMapAssoc.h" 20 | 21 | // myGN solver 22 | #include "factor/GyroAcceBiasFactorTMN.hpp" 23 | #include "factor/Point2PlaneFactorTMN.hpp" 24 | 25 | struct ImuIdx 26 | { 27 | ImuIdx(int &i_, int &j_, int &k_) 28 | : i(i_), j(j_), k(k_) {}; 29 | int i; int j; int k; 30 | }; 31 | 32 | struct lidarFeaIdx 33 | { 34 | lidarFeaIdx(int &widx_, int &pointidx_, int &depth_, int absidx_) 35 | : wdidx(widx_), pointidx(pointidx_), depth(depth_), absidx(absidx_) {}; 36 | 37 | int wdidx; int pointidx; int depth; int absidx; 38 | }; 39 | 40 | class tmnSolver 41 | { 42 | 43 | public: 44 | 45 | // Constructor 46 | tmnSolver(ros::NodeHandlePtr &nh_); 47 | // Destructor 48 | ~tmnSolver(); 49 | 50 | // Update the dimenstions of the optimization problem. 51 | void UpdateDimensions(int &imu_factors, int &ldr_factors, int knots); 52 | 53 | // Evaluate the imu factors to update the residual and jacobian 54 | void EvaluateImuFactors 55 | ( 56 | PoseSplineX &traj, vector &xr, vector &xp, Vector3d &xbg, Vector3d &xba, 57 | deque> &SwImuBundle, vector &imuSelected, ImuBias imuBiasPrior, 58 | VectorXd &r, MatrixXd &J, double* cost 59 | ); 60 | 61 | // Evaluate the lidar factors to update the residual and jacobian 62 | void EvaluateLdrFactors 63 | ( 64 | PoseSplineX &traj, vector &xr, vector &xp, 65 | deque &SwCloudDskDS, 66 | deque> &SwLidarCoef, 67 | vector &featureSelected, 68 | VectorXd &r, MatrixXd &J, double* cost 69 | ); 70 | 71 | // Evaluate the marginalization factors to update the residual and jacobian 72 | void EvaluatePriFactors 73 | ( 74 | int &iter, 75 | map &prev_knot_x, 76 | map &curr_knot_x, 77 | vector &xr, 78 | vector &xp, 79 | Vector3d &xbg, 80 | Vector3d &xba, 81 | SparseMatrix &bprior_sparse, 82 | SparseMatrix &Hprior_sparse, 83 | VectorXd* bprior_reduced, 84 | MatrixXd* Hprior_reduced, 85 | double* cost 86 | ); 87 | 88 | // Solve the problem 89 | bool Solve 90 | ( 91 | PoseSplineX &traj, 92 | Vector3d &BIG, 93 | Vector3d &BIA, 94 | map &prev_knot_x, 95 | map &curr_knot_x, 96 | int &swNextBase, 97 | int &iter, 98 | deque> &SwImuBundle, 99 | deque &SwCloudDskDS, 100 | deque> &SwLidarCoef, 101 | vector &imuSelected, 102 | vector &featureSelected, 103 | string &iekf_report, 104 | slict::OptStat &report, 105 | slict::TimeLog &tlog 106 | ); 107 | 108 | // Utility to convert between prior forms 109 | void HbToJr(const MatrixXd &H, const VectorXd &b, MatrixXd &J, VectorXd &r); 110 | 111 | // Update the priors when there is a relocalization event 112 | void RelocalizePrior(SE3d tf); 113 | 114 | private: 115 | 116 | // Node handle to get information needed 117 | ros::NodeHandlePtr nh; 118 | 119 | // gravity constant 120 | Vector3d GRAV; 121 | 122 | // IMU params 123 | double GYR_N = 1.0; 124 | double ACC_N = 1.0; 125 | double GYR_W = 1.0; 126 | double ACC_W = 1.0; 127 | 128 | // Lidar params 129 | double lidar_weight = 1.0; 130 | 131 | bool find_factor_cost = true; 132 | 133 | // Damping factor 134 | double lambda = 1.0; 135 | 136 | // Fuse marginalization 137 | bool fuse_marg = false; 138 | 139 | // Maximum change 140 | double dx_thres = 0.5; 141 | 142 | // Point to map association 143 | PointToMapAssoc *pma; 144 | 145 | // Maximum number of iteration 146 | int max_outer_iters; 147 | 148 | // Prior weight 149 | double prior_weight = 10; 150 | 151 | // Marginalization info 152 | MatrixXd Hkeep; 153 | VectorXd bkeep; 154 | MatrixXd Jm; // 155 | VectorXd rm; // 156 | // Marginalized states 157 | vector xse3_keep; 158 | Vector3d xbig_keep; 159 | Vector3d xbia_keep; 160 | MatrixXd dXkeep; 161 | 162 | // Index of knots in the marginalization hessian 163 | vector> knot_x_keep; 164 | 165 | int WINDOW_SIZE; 166 | int N_SUB_SEG; 167 | int SPLINE_N; 168 | 169 | }; -------------------------------------------------------------------------------- /srv/globalMapsPublish.srv: -------------------------------------------------------------------------------- 1 | uint8 downsample #to indicate if we want to downsample the maps before publishing 2 | --- 3 | uint8 result #result of the command, check for verbal reference in the 'srv_return_verbal.h' 4 | --------------------------------------------------------------------------------