├── README.md
└── SC-LIO-SAM
├── CMakeLists.txt
├── LICENSE
├── config
├── params_liosam.yaml
└── params_mulran.yaml
├── doc
├── kaist02.png
├── removert_eaxmple.png
├── riverside03.png
├── saver.png
└── utils_example.png
├── include
├── KDTreeVectorOfVectorsAdaptor.h
├── Scancontext.h
├── nanoflann.hpp
├── tictoc.h
└── utility.h
├── launch
├── include
│ ├── config
│ │ ├── robot.urdf.xacro
│ │ └── rviz.rviz
│ ├── module_loam.launch
│ ├── module_navsat.launch
│ ├── module_robot_state_publisher.launch
│ ├── module_rviz.launch
│ └── rosconsole
│ │ ├── rosconsole_error.conf
│ │ ├── rosconsole_info.conf
│ │ └── rosconsole_warn.conf
├── run.launch
└── run_mulran.launch
├── msg
└── cloud_info.msg
├── package.xml
├── src
├── Scancontext.cpp
├── featureExtraction.cpp
├── imageProjection.cpp
├── imuPreintegration.cpp
└── mapOptmization.cpp
└── tools
├── matlab
├── listdir.m
└── pcd_viewer.m
└── python
├── bone_table.npy
├── jet_table.npy
├── makeMergedMap.py
└── pypcdMyUtils.py
/README.md:
--------------------------------------------------------------------------------
1 | # SC-LIO-SAM
2 | version 2021-06-24
3 |
4 | ## What is SC-LIO-SAM?
5 | - SC-LIO-SAM is **a real-time lidar-inertial SLAM package.**
6 | - LiDAR-inertial SLAM: Scan Context + LIO-SAM
7 | - This repository is an example use-case of [Scan Context](https://github.com/irapkaist/scancontext), which is a fast and robust LiDAR place recognition method.
8 | - For more details for each algorithm please refer to nanoflann . nanoflann is an also single-header-program and that file is in our directory.
16 | - Easy to use: A user just remembers and uses only two API functions; ```makeAndSaveScancontextAndKeys``` and ```detectLoopClosureID```.
17 | - Fast: A single loop detection requires under 30 ms (for 20 x 60 size, 3 candidates)
18 |
19 |
20 | ## Examples
21 |
22 | We provide example results using [MulRan dataset](https://sites.google.com/view/mulran-pr/dataset), which provides LiDAR and 9dof IMU data. You can see the parameter file (i.e., params_mulran.yaml) modified for MulRan dataset.
23 |
24 | #### example 1: [KAIST02 of MulRan dataset](https://youtu.be/Il0YxztuZEU?t=280)
25 |
26 |
27 |
28 |
29 | #### example 2: [Riverside03 of MulRan dataset](https://youtu.be/Y6DXlC34qlc?t=459)
30 |
31 |
32 | - As seen in the above [video](https://youtu.be/Y6DXlC34qlc?t=459), the combination of Scan Context loop detector and LIO-SAM's odometry is robust to highly dynamic and less structured environments (e.g., a wide road on a bridge with many moving objects).
33 |
34 | ## How to use?
35 | - We provide a tutorial that runs SC-LIO-SAM on MulRan dataset, you can reproduce the above results by following these steps.
36 |
37 | 1. You can download the dataset at the [MulRan dataset website](https://sites.google.com/view/mulran-pr/home)
38 | 2. Place the directory `SC-LIO-SAM` under user catkin work space
69 |
70 | - If you use the above saved files, you can feed these data to [Removert](https://github.com/irapkaist/removert) and can removing dynamic objects. No GT labels or external sensor data such as RTK-GPS is required. This [tutorial](https://youtu.be/UiYYrPMcIRU) guides steps from running SC-LIO-SAM to save data to Removert to remove dynamic objects in a scan. Example results are:
71 |
72 |
73 | - For the safe and light-weight map saver, we support off-line scan merging utils for the global map construction within user's ROI (see tools/python/makeMergedMap.py, for the details, see the [tutorial video](https://youtu.be/jmR3DH_A4Co))
74 |
75 |
76 | ## Cite SC-LIO-SAM
77 |
78 | ```
79 | @INPROCEEDINGS { gkim-2018-iros,
80 | author = {Kim, Giseop and Kim, Ayoung},
81 | title = { Scan Context: Egocentric Spatial Descriptor for Place Recognition within {3D} Point Cloud Map },
82 | booktitle = { Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems },
83 | year = { 2018 },
84 | month = { Oct. },
85 | address = { Madrid }
86 | }
87 | ```
88 | and
89 | ```
90 | @inproceedings{liosam2020shan,
91 | title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
92 | author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
93 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
94 | pages={5135-5142},
95 | year={2020},
96 | organization={IEEE}
97 | }
98 | ```
99 |
100 | ## Contact
101 | - Maintainer: Giseop Kim (`paulgkim@kaist.ac.kr`)
102 | #### Contributors
103 | - Minwoo Jung: made the original LIO-SAM runs on the MulRan dataset.
104 |
105 | ## Acknowledgement
106 | - SC-LIO-SAM is based on LIO-SAM (Tixiao Shan et al., IROS 2020). We thank Tixiao Shan and the LIO-SAM authors for providing a great base framework.
107 |
108 | ## Update history
109 | - 2021.06.23
110 | - yaml file is reformatted to support the compatible form with the recent original LIO-SAM repository.
111 | - offline ROI global map construction python util is supported.
112 |
113 | ## TODO
114 | - About performance
115 | - improve better RS loop closing (ICP test sometimes fails in reverse directions)
116 | - support reverse-closing of SC loops with Scan Context initial yaw
117 | - support SC augmentation
118 | - lagged RS loop closing after large drifts solved
119 | - About funtions for convenience
120 | - [x] save extended data: nodes' time, 6D pose, node's point cloud, and corresponding SC descriptors
121 | - [x] make a static map and self-labeled dynamic points by combining SC-LIO-SAM and [removert](https://github.com/irapkaist/removert).
122 | - Minor (not related to the performance)
123 | - fix the visualization issue: disappearing map points after SC loop closing
124 | - fix safe-save for map points after closing the program
125 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(lio_sam)
3 |
4 | set(CMAKE_BUILD_TYPE "Release")
5 | # set(CMAKE_CXX_FLAGS "-std=c++14")
6 | set(CMAKE_CXX_STANDARD 14)
7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread")
8 |
9 | find_package(catkin REQUIRED COMPONENTS
10 | tf
11 | roscpp
12 | rospy
13 | cv_bridge
14 | # pcl library
15 | pcl_conversions
16 | # msgs
17 | std_msgs
18 | sensor_msgs
19 | geometry_msgs
20 | nav_msgs
21 | message_generation
22 | )
23 |
24 | find_package(OpenMP REQUIRED)
25 | find_package(PCL REQUIRED QUIET)
26 | find_package(OpenCV REQUIRED QUIET)
27 | find_package(GTSAM REQUIRED QUIET)
28 |
29 | add_message_files(
30 | DIRECTORY msg
31 | FILES
32 | cloud_info.msg
33 | )
34 |
35 | generate_messages(
36 | DEPENDENCIES
37 | geometry_msgs
38 | std_msgs
39 | nav_msgs
40 | sensor_msgs
41 | )
42 |
43 | catkin_package(
44 | INCLUDE_DIRS include
45 | DEPENDS PCL GTSAM
46 |
47 | CATKIN_DEPENDS
48 | std_msgs
49 | nav_msgs
50 | geometry_msgs
51 | sensor_msgs
52 | message_runtime
53 | message_generation
54 | )
55 |
56 | # include directories
57 | include_directories(
58 | include
59 | ${catkin_INCLUDE_DIRS}
60 | ${PCL_INCLUDE_DIRS}
61 | ${OpenCV_INCLUDE_DIRS}
62 | ${GTSAM_INCLUDE_DIR}
63 | )
64 |
65 | # link directories
66 | link_directories(
67 | include
68 | ${PCL_LIBRARY_DIRS}
69 | ${OpenCV_LIBRARY_DIRS}
70 | ${GTSAM_LIBRARY_DIRS}
71 | )
72 |
73 | ###########
74 | ## Build ##
75 | ###########
76 |
77 | # Range Image Projection
78 | add_executable(${PROJECT_NAME}_imageProjection src/imageProjection.cpp)
79 | add_dependencies(${PROJECT_NAME}_imageProjection ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
80 | target_link_libraries(${PROJECT_NAME}_imageProjection ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
81 |
82 | # Feature Association
83 | add_executable(${PROJECT_NAME}_featureExtraction src/featureExtraction.cpp)
84 | add_dependencies(${PROJECT_NAME}_featureExtraction ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
85 | target_link_libraries(${PROJECT_NAME}_featureExtraction ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES})
86 |
87 | # Mapping Optimization
88 | add_executable(${PROJECT_NAME}_mapOptmization
89 | src/mapOptmization.cpp
90 | src/Scancontext.cpp
91 | )
92 | add_dependencies(${PROJECT_NAME}_mapOptmization ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
93 | target_compile_options(${PROJECT_NAME}_mapOptmization PRIVATE ${OpenMP_CXX_FLAGS})
94 | target_link_libraries(${PROJECT_NAME}_mapOptmization ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${OpenMP_CXX_FLAGS} gtsam)
95 |
96 | # IMU Preintegration
97 | add_executable(${PROJECT_NAME}_imuPreintegration src/imuPreintegration.cpp)
98 | target_link_libraries(${PROJECT_NAME}_imuPreintegration ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} gtsam)
--------------------------------------------------------------------------------
/SC-LIO-SAM/LICENSE:
--------------------------------------------------------------------------------
1 | BSD 3-Clause License
2 |
3 | Copyright (c) 2020, Tixiao Shan
4 | All rights reserved.
5 |
6 | Redistribution and use in source and binary forms, with or without
7 | modification, are permitted provided that the following conditions are met:
8 |
9 | * Redistributions of source code must retain the above copyright notice, this
10 | list of conditions and the following disclaimer.
11 |
12 | * Redistributions in binary form must reproduce the above copyright notice,
13 | this list of conditions and the following disclaimer in the documentation
14 | and/or other materials provided with the distribution.
15 |
16 | * Neither the name of the copyright holder nor the names of its
17 | contributors may be used to endorse or promote products derived from
18 | this software without specific prior written permission.
19 |
20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/config/params_liosam.yaml:
--------------------------------------------------------------------------------
1 | lio_sam:
2 |
3 | # Topics
4 | pointCloudTopic: "/os_cloud_node/points" # Point cloud data
5 | imuTopic: "/imu/data" # IMU data
6 | odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
7 | gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
8 |
9 | # Frames
10 | lidarFrame: "base_link"
11 | baselinkFrame: "base_link"
12 | odometryFrame: "odom"
13 | mapFrame: "map"
14 |
15 | # GPS Settings
16 | useImuHeadingInitialization: true # if using GPS data, set to "true"
17 | useGpsElevation: false # if GPS elevation is bad, set to "false"
18 | gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
19 | poseCovThreshold: 25.0 # m^2, threshold for using GPS data
20 |
21 | # Export settings
22 | savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3
23 | savePCDDirectory: "/home/user/Desktop/scliosam/data/" # use global path, and end with "/"
24 | # warning: if you have already data in the above savePCDDirectory, it will all remove and remake them. Thus, backup is recommended if pre-made data exist.
25 |
26 | # Sensor Settings
27 | sensor: ouster # lidar sensor type, either 'velodyne' or 'ouster'
28 | N_SCAN: 128 # number of lidar channel (i.e., 16, 32, 64, 128)
29 | Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
30 | downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
31 | lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
32 | lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
33 |
34 | # IMU Settings
35 | imuAccNoise: 0.009939570888238808e-03
36 | imuGyrNoise: 0.005636343949698187e-03
37 | imuAccBiasN: 0.64356659353532566e-03
38 | imuGyrBiasN: 0.35640318696367613e-03
39 | imuGravity: 9.80511
40 | imuRPYWeight: 0.01
41 |
42 | # Extrinsics (lidar -> IMU)
43 | extrinsicTrans: [-0.1, 0.0, 0.0]
44 | extrinsicRot: [1, 0, 0,
45 | 0, 1, 0,
46 | 0, 0, 1]
47 | extrinsicRPY: [1, 0, 0,
48 | 0, 1, 0,
49 | 0, 0, 1]
50 | # extrinsicRot: [1, 0, 0,
51 | # 0, 1, 0,
52 | # 0, 0, 1]
53 | # extrinsicRPY: [1, 0, 0,
54 | # 0, 1, 0,
55 | # 0, 0, 1]
56 |
57 | # LOAM feature threshold
58 | edgeThreshold: 1.0
59 | surfThreshold: 0.1
60 | edgeFeatureMinValidNum: 10
61 | surfFeatureMinValidNum: 100
62 |
63 | # voxel filter paprams
64 | odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
65 | mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor
66 | mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
67 |
68 | # robot motion constraint (in case you are using a 2D robot)
69 | z_tollerance: 1000 # meters
70 | rotation_tollerance: 1000 # radians
71 |
72 | # CPU Params
73 | numberOfCores: 4 # number of cores for mapping optimization
74 | mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
75 |
76 | # Surrounding map
77 | surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
78 | surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
79 | surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
80 | surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
81 |
82 | # Loop closure
83 | loopClosureEnableFlag: true
84 | loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency
85 | surroundingKeyframeSize: 50 # submap size (when loop closure enabled)
86 | historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
87 | historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
88 | historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
89 | historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
90 |
91 | # Visualization
92 | globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
93 | globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
94 | globalMapVisualizationLeafSize: 0.2 # meters, global map visualization cloud density
95 |
96 |
97 |
98 |
99 | # Navsat (convert GPS coordinates to Cartesian)
100 | navsat:
101 | frequency: 50
102 | wait_for_datum: false
103 | delay: 0.0
104 | magnetic_declination_radians: 0
105 | yaw_offset: 0
106 | zero_altitude: true
107 | broadcast_utm_transform: false
108 | broadcast_utm_transform_as_parent_frame: false
109 | publish_filtered_gps: false
110 |
111 | # EKF for Navsat
112 | ekf_gps:
113 | publish_tf: false
114 | map_frame: map
115 | odom_frame: odom
116 | base_link_frame: base_link
117 | world_frame: odom
118 |
119 | frequency: 50
120 | two_d_mode: false
121 | sensor_timeout: 0.01
122 | # -------------------------------------
123 | # External IMU:
124 | # -------------------------------------
125 | imu0: imu_correct
126 | # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
127 | imu0_config: [false, false, false,
128 | true, true, true,
129 | false, false, false,
130 | false, false, true,
131 | true, true, true]
132 | imu0_differential: false
133 | imu0_queue_size: 50
134 | imu0_remove_gravitational_acceleration: true
135 | # -------------------------------------
136 | # Odometry (From Navsat):
137 | # -------------------------------------
138 | odom0: odometry/gps
139 | odom0_config: [true, true, true,
140 | false, false, false,
141 | false, false, false,
142 | false, false, false,
143 | false, false, false]
144 | odom0_differential: false
145 | odom0_queue_size: 10
146 |
147 | # x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot
148 | process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
149 | 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
150 | 0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
151 | 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
152 | 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
153 | 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
154 | 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0,
155 | 0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0,
156 | 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
157 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
158 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
159 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0,
160 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
161 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
162 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
163 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/config/params_mulran.yaml:
--------------------------------------------------------------------------------
1 | lio_sam:
2 |
3 | # Topics
4 | pointCloudTopic: "/os1_points" # Point cloud data
5 | imuTopic: "/imu/data_raw" # IMU data
6 | odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
7 | gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
8 |
9 | # Frames
10 | lidarFrame: "base_link"
11 | baselinkFrame: "base_link"
12 | odometryFrame: "odom"
13 | mapFrame: "map"
14 |
15 | # GPS Settings
16 | useImuHeadingInitialization: true # if using GPS data, set to "true"
17 | useGpsElevation: false # if GPS elevation is bad, set to "false"
18 | gpsCovThreshold: 2.0 # m^2, threshold for using GPS data
19 | poseCovThreshold: 25.0 # m^2, threshold for using GPS data
20 |
21 | # Export settings
22 | savePCD: true # https://github.com/TixiaoShan/LIO-SAM/issues/3
23 | savePCDDirectory: "/home/user/Desktop/scliosam/data/" # use global path, and end with "/"
24 | # warning: if you have already data in the above savePCDDirectory, it will all remove and remake them. Thus, backup is recommended if pre-made data exist.
25 |
26 | # Sensor Settings
27 | sensor: mulran # lidar sensor type, either 'velodyne' or 'ouster'
28 | N_SCAN: 64 # number of lidar channel (i.e., 16, 32, 64, 128)
29 | Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
30 | downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
31 | lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
32 | lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
33 |
34 | # IMU Settings
35 | imuAccNoise: 0.009939570888238808e-03
36 | imuGyrNoise: 0.005636343949698187e-03
37 | imuAccBiasN: 0.64356659353532566e-03
38 | imuGyrBiasN: 0.35640318696367613e-03
39 | imuGravity: 9.80511
40 | imuRPYWeight: 0.01
41 |
42 | # Extrinsics (lidar -> IMU, of MulRan dataset)
43 | extrinsicTrans: [1.77, -0.00, -0.05]
44 | extrinsicRot: [-1, 0, 0,
45 | 0, -1, 0,
46 | 0, 0, 1]
47 | extrinsicRPY: [-1, 0, 0,
48 | 0, -1, 0,
49 | 0, 0, 1]
50 | # extrinsicRot: [1, 0, 0,
51 | # 0, 1, 0,
52 | # 0, 0, 1]
53 | # extrinsicRPY: [1, 0, 0,
54 | # 0, 1, 0,
55 | # 0, 0, 1]
56 |
57 | # LOAM feature threshold
58 | edgeThreshold: 1.0
59 | surfThreshold: 0.1
60 | edgeFeatureMinValidNum: 10
61 | surfFeatureMinValidNum: 100
62 |
63 | # voxel filter paprams
64 | odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
65 | mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor
66 | mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
67 |
68 | # robot motion constraint (in case you are using a 2D robot)
69 | z_tollerance: 1000 # meters
70 | rotation_tollerance: 1000 # radians
71 |
72 | # CPU Params
73 | numberOfCores: 4 # number of cores for mapping optimization
74 | mappingProcessInterval: 0.15 # seconds, regulate mapping frequency
75 |
76 | # Surrounding map
77 | surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold
78 | surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold
79 | surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses
80 | surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
81 |
82 | # Loop closure
83 | loopClosureEnableFlag: true
84 | loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency
85 | surroundingKeyframeSize: 50 # submap size (when loop closure enabled)
86 | historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure
87 | historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure
88 | historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure
89 | historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
90 |
91 | # Visualization
92 | globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius
93 | globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density
94 | globalMapVisualizationLeafSize: 0.2 # meters, global map visualization cloud density
95 |
96 |
97 |
98 |
99 | # Navsat (convert GPS coordinates to Cartesian)
100 | navsat:
101 | frequency: 50
102 | wait_for_datum: false
103 | delay: 0.0
104 | magnetic_declination_radians: 0
105 | yaw_offset: 0
106 | zero_altitude: true
107 | broadcast_utm_transform: false
108 | broadcast_utm_transform_as_parent_frame: false
109 | publish_filtered_gps: false
110 |
111 | # EKF for Navsat
112 | ekf_gps:
113 | publish_tf: false
114 | map_frame: map
115 | odom_frame: odom
116 | base_link_frame: base_link
117 | world_frame: odom
118 |
119 | frequency: 50
120 | two_d_mode: false
121 | sensor_timeout: 0.01
122 | # -------------------------------------
123 | # External IMU:
124 | # -------------------------------------
125 | imu0: imu_correct
126 | # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
127 | imu0_config: [false, false, false,
128 | true, true, true,
129 | false, false, false,
130 | false, false, true,
131 | true, true, true]
132 | imu0_differential: false
133 | imu0_queue_size: 50
134 | imu0_remove_gravitational_acceleration: true
135 | # -------------------------------------
136 | # Odometry (From Navsat):
137 | # -------------------------------------
138 | odom0: odometry/gps
139 | odom0_config: [true, true, true,
140 | false, false, false,
141 | false, false, false,
142 | false, false, false,
143 | false, false, false]
144 | odom0_differential: false
145 | odom0_queue_size: 10
146 |
147 | # x y z r p y x_dot y_dot z_dot r_dot p_dot y_dot x_ddot y_ddot z_ddot
148 | process_noise_covariance: [ 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
149 | 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
150 | 0, 0, 10.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
151 | 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
152 | 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
153 | 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
154 | 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0,
155 | 0, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0,
156 | 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
157 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
158 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
159 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0,
160 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
161 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
162 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]
163 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/doc/kaist02.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-LIO-SAM/d43ca00d97a756303c10975e32e8d66bfabb337d/SC-LIO-SAM/doc/kaist02.png
--------------------------------------------------------------------------------
/SC-LIO-SAM/doc/removert_eaxmple.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-LIO-SAM/d43ca00d97a756303c10975e32e8d66bfabb337d/SC-LIO-SAM/doc/removert_eaxmple.png
--------------------------------------------------------------------------------
/SC-LIO-SAM/doc/riverside03.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-LIO-SAM/d43ca00d97a756303c10975e32e8d66bfabb337d/SC-LIO-SAM/doc/riverside03.png
--------------------------------------------------------------------------------
/SC-LIO-SAM/doc/saver.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-LIO-SAM/d43ca00d97a756303c10975e32e8d66bfabb337d/SC-LIO-SAM/doc/saver.png
--------------------------------------------------------------------------------
/SC-LIO-SAM/doc/utils_example.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-LIO-SAM/d43ca00d97a756303c10975e32e8d66bfabb337d/SC-LIO-SAM/doc/utils_example.png
--------------------------------------------------------------------------------
/SC-LIO-SAM/include/KDTreeVectorOfVectorsAdaptor.h:
--------------------------------------------------------------------------------
1 | /***********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * 1. Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * 2. Redistributions in binary form must reproduce the above copyright
14 | * notice, this list of conditions and the following disclaimer in the
15 | * documentation and/or other materials provided with the distribution.
16 | *
17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 | *************************************************************************/
28 |
29 | #pragma once
30 |
31 | #include
32 |
33 | #include
34 |
35 | // ===== This example shows how to use nanoflann with these types of containers: =======
36 | //typedef std::vector > my_vector_of_vectors_t;
37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include
38 | // =====================================================================================
39 |
40 |
41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
42 | * The i'th vector represents a point in the state space.
43 | *
44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
45 | * \tparam num_t The type of the point coordinates (typically, double or float).
46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
48 | */
49 | template
50 | struct KDTreeVectorOfVectorsAdaptor
51 | {
52 | typedef KDTreeVectorOfVectorsAdaptor self_t;
53 | typedef typename Distance::template traits::distance_t metric_t;
54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
55 |
56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
57 |
58 | /// Constructor: takes a const ref to the vector of vectors object with the data points
59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
60 | {
61 | assert(mat.size() != 0 && mat[0].size() != 0);
62 | const size_t dims = mat[0].size();
63 | if (DIM>0 && static_cast(dims) != DIM)
64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
66 | index->buildIndex();
67 | }
68 |
69 | ~KDTreeVectorOfVectorsAdaptor() {
70 | delete index;
71 | }
72 |
73 | const VectorOfVectorsType &m_data;
74 |
75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
76 | * Note that this is a short-cut method for index->findNeighbors().
77 | * The user can also call index->... methods as desired.
78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
79 | */
80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
81 | {
82 | nanoflann::KNNResultSet resultSet(num_closest);
83 | resultSet.init(out_indices, out_distances_sq);
84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
85 | }
86 |
87 | /** @name Interface expected by KDTreeSingleIndexAdaptor
88 | * @{ */
89 |
90 | const self_t & derived() const {
91 | return *this;
92 | }
93 | self_t & derived() {
94 | return *this;
95 | }
96 |
97 | // Must return the number of data points
98 | inline size_t kdtree_get_point_count() const {
99 | return m_data.size();
100 | }
101 |
102 | // Returns the dim'th component of the idx'th point in the class:
103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
104 | return m_data[idx][dim];
105 | }
106 |
107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop.
108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
110 | template
111 | bool kdtree_get_bbox(BBOX & /*bb*/) const {
112 | return false;
113 | }
114 |
115 | /** @} */
116 |
117 | }; // end of KDTreeVectorOfVectorsAdaptor
118 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/include/Scancontext.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | #include
14 |
15 | #include
16 | #include
17 | #include
18 | #include
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 |
25 | #include "nanoflann.hpp"
26 | #include "KDTreeVectorOfVectorsAdaptor.h"
27 |
28 | #include "tictoc.h"
29 |
30 | using namespace Eigen;
31 | using namespace nanoflann;
32 |
33 | using std::cout;
34 | using std::endl;
35 | using std::make_pair;
36 |
37 | using std::atan2;
38 | using std::cos;
39 | using std::sin;
40 |
41 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context)
42 | using KeyMat = std::vector >;
43 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >;
44 |
45 |
46 | // namespace SC2
47 | // {
48 |
49 | void coreImportTest ( void );
50 |
51 |
52 | // sc param-independent helper functions
53 | float xy2theta( const float & _x, const float & _y );
54 | MatrixXd circshift( MatrixXd &_mat, int _num_shift );
55 | std::vector eig2stdvec( MatrixXd _eigmat );
56 |
57 |
58 | class SCManager
59 | {
60 | public:
61 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.
62 |
63 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down );
64 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc );
65 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc );
66 |
67 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 );
68 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18)
69 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18)
70 |
71 | // User-side API
72 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down );
73 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw
74 |
75 | // for ltmapper
76 | const Eigen::MatrixXd& getConstRefRecentSCD(void);
77 |
78 | public:
79 | // hyper parameters ()
80 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
81 |
82 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
83 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
84 | const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
85 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
86 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);
87 |
88 | // tree
89 | const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok.
90 | const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper)
91 |
92 | // loop thres
93 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
94 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
95 | const double SC_DIST_THRES = 0.3; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
96 | // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
97 |
98 | // config
99 | const int TREE_MAKING_PERIOD_ = 10; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N.
100 | int tree_making_period_conter = 0;
101 |
102 | // data
103 | std::vector polarcontexts_timestamp_; // optional.
104 | std::vector polarcontexts_;
105 | std::vector polarcontext_invkeys_;
106 | std::vector polarcontext_vkeys_;
107 |
108 | KeyMat polarcontext_invkeys_mat_;
109 | KeyMat polarcontext_invkeys_to_search_;
110 | std::unique_ptr polarcontext_tree_;
111 |
112 | }; // SCManager
113 |
114 | // } // namespace SC2
115 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/include/tictoc.h:
--------------------------------------------------------------------------------
1 | // Author: Tong Qin qintonguav@gmail.com
2 | // Shaozu Cao saozu.cao@connect.ust.hk
3 |
4 | #pragma once
5 |
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 |
12 | class TicToc
13 | {
14 | public:
15 | TicToc()
16 | {
17 | tic();
18 | }
19 |
20 | TicToc( bool _disp )
21 | {
22 | disp_ = _disp;
23 | tic();
24 | }
25 |
26 | void tic()
27 | {
28 | start = std::chrono::system_clock::now();
29 | }
30 |
31 | void toc( std::string _about_task )
32 | {
33 | end = std::chrono::system_clock::now();
34 | std::chrono::duration elapsed_seconds = end - start;
35 | double elapsed_ms = elapsed_seconds.count() * 1000;
36 |
37 | if( disp_ )
38 | {
39 | std::cout.precision(3); // 10 for sec, 3 for ms
40 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl;
41 | }
42 | }
43 |
44 | private:
45 | std::chrono::time_point start, end;
46 | bool disp_ = false;
47 | };
48 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/include/utility.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 |
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 |
15 | #include
16 |
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | #include
26 | #include
27 | #include
28 | #include
29 | #include
30 |
31 | #include
32 | #include
33 | #include
34 | #include
35 |
36 | #include
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 | #include
52 | #include
53 | #include
54 | #include
55 |
56 | using namespace std;
57 |
58 | typedef std::numeric_limits< double > dbl;
59 |
60 | typedef pcl::PointXYZI PointType;
61 |
62 | enum class SensorType { MULRAN, VELODYNE, OUSTER };
63 |
64 | class ParamServer
65 | {
66 | public:
67 |
68 | ros::NodeHandle nh;
69 |
70 | std::string robot_id;
71 |
72 | //Topics
73 | string pointCloudTopic;
74 | string imuTopic;
75 | string odomTopic;
76 | string gpsTopic;
77 |
78 | //Frames
79 | string lidarFrame;
80 | string baselinkFrame;
81 | string odometryFrame;
82 | string mapFrame;
83 |
84 | // GPS Settings
85 | bool useImuHeadingInitialization;
86 | bool useGpsElevation;
87 | float gpsCovThreshold;
88 | float poseCovThreshold;
89 |
90 | // Save pcd
91 | bool savePCD;
92 | string savePCDDirectory;
93 |
94 | // Velodyne Sensor Configuration: Velodyne
95 | SensorType sensor;
96 | int N_SCAN;
97 | int Horizon_SCAN;
98 | int downsampleRate;
99 | float lidarMinRange;
100 | float lidarMaxRange;
101 |
102 | // IMU
103 | float imuAccNoise;
104 | float imuGyrNoise;
105 | float imuAccBiasN;
106 | float imuGyrBiasN;
107 | float imuGravity;
108 | float imuRPYWeight;
109 | vector extRotV;
110 | vector extRPYV;
111 | vector extTransV;
112 | Eigen::Matrix3d extRot;
113 | Eigen::Matrix3d extRPY;
114 | Eigen::Vector3d extTrans;
115 | Eigen::Quaterniond extQRPY;
116 |
117 | // LOAM
118 | float edgeThreshold;
119 | float surfThreshold;
120 | int edgeFeatureMinValidNum;
121 | int surfFeatureMinValidNum;
122 |
123 | // voxel filter paprams
124 | float odometrySurfLeafSize;
125 | float mappingCornerLeafSize;
126 | float mappingSurfLeafSize ;
127 |
128 | float z_tollerance;
129 | float rotation_tollerance;
130 |
131 | // CPU Params
132 | int numberOfCores;
133 | double mappingProcessInterval;
134 |
135 | // Surrounding map
136 | float surroundingkeyframeAddingDistThreshold;
137 | float surroundingkeyframeAddingAngleThreshold;
138 | float surroundingKeyframeDensity;
139 | float surroundingKeyframeSearchRadius;
140 |
141 | // Loop closure
142 | bool loopClosureEnableFlag;
143 | float loopClosureFrequency;
144 | int surroundingKeyframeSize;
145 | float historyKeyframeSearchRadius;
146 | float historyKeyframeSearchTimeDiff;
147 | int historyKeyframeSearchNum;
148 | float historyKeyframeFitnessScore;
149 |
150 | // global map visualization radius
151 | float globalMapVisualizationSearchRadius;
152 | float globalMapVisualizationPoseDensity;
153 | float globalMapVisualizationLeafSize;
154 |
155 | ParamServer()
156 | {
157 | nh.param("/robot_id", robot_id, "roboat");
158 |
159 | nh.param("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
160 | nh.param("lio_sam/imuTopic", imuTopic, "imu_correct");
161 | nh.param("lio_sam/odomTopic", odomTopic, "odometry/imu");
162 | nh.param("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
163 |
164 | nh.param("lio_sam/lidarFrame", lidarFrame, "base_link");
165 | nh.param("lio_sam/baselinkFrame", baselinkFrame, "base_link");
166 | nh.param("lio_sam/odometryFrame", odometryFrame, "odom");
167 | nh.param("lio_sam/mapFrame", mapFrame, "map");
168 |
169 | nh.param("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
170 | nh.param("lio_sam/useGpsElevation", useGpsElevation, false);
171 | nh.param("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
172 | nh.param("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
173 |
174 | nh.param("lio_sam/savePCD", savePCD, false);
175 | nh.param("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
176 |
177 | std::string sensorStr;
178 | nh.param("lio_sam/sensor", sensorStr, "");
179 | if (sensorStr == "velodyne")
180 | {
181 | sensor = SensorType::VELODYNE;
182 | }
183 | else if (sensorStr == "ouster")
184 | {
185 | sensor = SensorType::OUSTER;
186 | }
187 | else if (sensorStr == "mulran")
188 | {
189 | sensor = SensorType::MULRAN;
190 | }
191 | else
192 | {
193 | ROS_ERROR_STREAM(
194 | "Invalid sensor type (must be either 'velodyne' or 'ouster' or 'mulran'): " << sensorStr);
195 | ros::shutdown();
196 | }
197 |
198 | nh.param("lio_sam/N_SCAN", N_SCAN, 16);
199 | nh.param("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
200 | nh.param("lio_sam/downsampleRate", downsampleRate, 1);
201 | nh.param("lio_sam/lidarMinRange", lidarMinRange, 1.0);
202 | nh.param("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);
203 |
204 | nh.param("lio_sam/imuAccNoise", imuAccNoise, 0.01);
205 | nh.param("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
206 | nh.param("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
207 | nh.param("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
208 | nh.param("lio_sam/imuGravity", imuGravity, 9.80511);
209 | nh.param("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
210 | nh.param>("lio_sam/extrinsicRot", extRotV, vector());
211 | nh.param>("lio_sam/extrinsicRPY", extRPYV, vector());
212 | nh.param>("lio_sam/extrinsicTrans", extTransV, vector());
213 | extRot = Eigen::Map>(extRotV.data(), 3, 3);
214 | extRPY = Eigen::Map>(extRPYV.data(), 3, 3);
215 | extTrans = Eigen::Map>(extTransV.data(), 3, 1);
216 | extQRPY = Eigen::Quaterniond(extRPY);
217 |
218 | nh.param("lio_sam/edgeThreshold", edgeThreshold, 0.1);
219 | nh.param("lio_sam/surfThreshold", surfThreshold, 0.1);
220 | nh.param("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
221 | nh.param("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
222 |
223 | nh.param("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
224 | nh.param("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
225 | nh.param("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
226 |
227 | nh.param("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
228 | nh.param("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
229 |
230 | nh.param("lio_sam/numberOfCores", numberOfCores, 2);
231 | nh.param("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
232 |
233 | nh.param("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
234 | nh.param("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
235 | nh.param("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
236 | nh.param("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
237 |
238 | nh.param("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
239 | nh.param("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);
240 | nh.param("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
241 | nh.param("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
242 | nh.param("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
243 | nh.param("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
244 | nh.param("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
245 |
246 | nh.param("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
247 | nh.param("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
248 | nh.param("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
249 |
250 | usleep(100);
251 | }
252 |
253 | sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
254 | {
255 | sensor_msgs::Imu imu_out = imu_in;
256 | // rotate acceleration
257 | Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
258 | acc = extRot * acc;
259 | imu_out.linear_acceleration.x = acc.x();
260 | imu_out.linear_acceleration.y = acc.y();
261 | imu_out.linear_acceleration.z = acc.z();
262 | // rotate gyroscope
263 | Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
264 | gyr = extRot * gyr;
265 | imu_out.angular_velocity.x = gyr.x();
266 | imu_out.angular_velocity.y = gyr.y();
267 | imu_out.angular_velocity.z = gyr.z();
268 | // rotate roll pitch yaw
269 | Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
270 | Eigen::Quaterniond q_final = q_from * extQRPY;
271 | imu_out.orientation.x = q_final.x();
272 | imu_out.orientation.y = q_final.y();
273 | imu_out.orientation.z = q_final.z();
274 | imu_out.orientation.w = q_final.w();
275 |
276 | if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
277 | {
278 | ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
279 | ros::shutdown();
280 | }
281 |
282 | return imu_out;
283 | }
284 | };
285 |
286 |
287 | sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
288 | {
289 | sensor_msgs::PointCloud2 tempCloud;
290 | pcl::toROSMsg(*thisCloud, tempCloud);
291 | tempCloud.header.stamp = thisStamp;
292 | tempCloud.header.frame_id = thisFrame;
293 | if (thisPub->getNumSubscribers() != 0)
294 | thisPub->publish(tempCloud);
295 | return tempCloud;
296 | }
297 |
298 | template
299 | double ROS_TIME(T msg)
300 | {
301 | return msg->header.stamp.toSec();
302 | }
303 |
304 |
305 | template
306 | void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
307 | {
308 | *angular_x = thisImuMsg->angular_velocity.x;
309 | *angular_y = thisImuMsg->angular_velocity.y;
310 | *angular_z = thisImuMsg->angular_velocity.z;
311 | }
312 |
313 |
314 | template
315 | void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
316 | {
317 | *acc_x = thisImuMsg->linear_acceleration.x;
318 | *acc_y = thisImuMsg->linear_acceleration.y;
319 | *acc_z = thisImuMsg->linear_acceleration.z;
320 | }
321 |
322 |
323 | template
324 | void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
325 | {
326 | double imuRoll, imuPitch, imuYaw;
327 | tf::Quaternion orientation;
328 | tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
329 | tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
330 |
331 | *rosRoll = imuRoll;
332 | *rosPitch = imuPitch;
333 | *rosYaw = imuYaw;
334 | }
335 |
336 |
337 | float pointDistance(PointType p)
338 | {
339 | return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
340 | }
341 |
342 |
343 | float pointDistance(PointType p1, PointType p2)
344 | {
345 | return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
346 | }
347 |
348 | void saveSCD(std::string fileName, Eigen::MatrixXd matrix, std::string delimiter = " ")
349 | {
350 | // delimiter: ", " or " " etc.
351 |
352 | int precision = 3; // or Eigen::FullPrecision, but SCD does not require such accruate precisions so 3 is enough.
353 | const static Eigen::IOFormat the_format(precision, Eigen::DontAlignCols, delimiter, "\n");
354 |
355 | std::ofstream file(fileName);
356 | if (file.is_open())
357 | {
358 | file << matrix.format(the_format);
359 | file.close();
360 | }
361 | }
362 |
363 | std::string padZeros(int val, int num_digits = 6) {
364 | std::ostringstream out;
365 | out << std::internal << std::setfill('0') << std::setw(num_digits) << val;
366 | return out.str();
367 | }
368 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/config/robot.urdf.xacro:
--------------------------------------------------------------------------------
1 |
2 |
3 |
9 |
13 |
14 |
16 |
20 |
21 |
23 |
27 |
28 |
30 |
34 |
35 |
36 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/config/rviz.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz/Displays
3 | Help Height: 191
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /Mapping1
8 | Splitter Ratio: 0.47826087474823
9 | Tree Height: 701
10 | - Class: rviz/Selection
11 | Name: Selection
12 | - Class: rviz/Views
13 | Expanded:
14 | - /Current View1
15 | Name: Views
16 | Splitter Ratio: 0.5
17 | - Class: rviz/Tool Properties
18 | Expanded: ~
19 | Name: Tool Properties
20 | Splitter Ratio: 0.5
21 | Preferences:
22 | PromptSaveOnExit: true
23 | Toolbars:
24 | toolButtonStyle: 2
25 | Visualization Manager:
26 | Class: ""
27 | Displays:
28 | - Class: rviz/Group
29 | Displays:
30 | - Class: rviz/TF
31 | Enabled: false
32 | Frame Timeout: 999
33 | Frames:
34 | All Enabled: false
35 | Marker Scale: 3
36 | Name: TF
37 | Show Arrows: true
38 | Show Axes: true
39 | Show Names: true
40 | Tree:
41 | {}
42 | Update Interval: 0
43 | Value: false
44 | - Class: rviz/Axes
45 | Enabled: true
46 | Length: 2
47 | Name: map
48 | Radius: 0.5
49 | Reference Frame: map
50 | Value: true
51 | - Class: rviz/Axes
52 | Enabled: true
53 | Length: 1
54 | Name: base_link
55 | Radius: 0.30000001192092896
56 | Reference Frame: base_link
57 | Value: true
58 | Enabled: true
59 | Name: links
60 | - Class: rviz/Group
61 | Displays:
62 | - Angle Tolerance: 0.10000000149011612
63 | Class: rviz/Odometry
64 | Covariance:
65 | Orientation:
66 | Alpha: 0.5
67 | Color: 255; 255; 127
68 | Color Style: Unique
69 | Frame: Local
70 | Offset: 1
71 | Scale: 1
72 | Value: true
73 | Position:
74 | Alpha: 0.30000001192092896
75 | Color: 204; 51; 204
76 | Scale: 1
77 | Value: true
78 | Value: false
79 | Enabled: true
80 | Keep: 300
81 | Name: Odom IMU
82 | Position Tolerance: 0.10000000149011612
83 | Shape:
84 | Alpha: 1
85 | Axes Length: 0.5
86 | Axes Radius: 0.10000000149011612
87 | Color: 255; 25; 0
88 | Head Length: 0.30000001192092896
89 | Head Radius: 0.10000000149011612
90 | Shaft Length: 1
91 | Shaft Radius: 0.05000000074505806
92 | Value: Axes
93 | Topic: /odometry/imu
94 | Unreliable: false
95 | Value: true
96 | - Angle Tolerance: 0.10000000149011612
97 | Class: rviz/Odometry
98 | Covariance:
99 | Orientation:
100 | Alpha: 0.5
101 | Color: 255; 255; 127
102 | Color Style: Unique
103 | Frame: Local
104 | Offset: 1
105 | Scale: 1
106 | Value: true
107 | Position:
108 | Alpha: 0.30000001192092896
109 | Color: 204; 51; 204
110 | Scale: 1
111 | Value: true
112 | Value: false
113 | Enabled: true
114 | Keep: 300
115 | Name: Odom GPS
116 | Position Tolerance: 0.10000000149011612
117 | Shape:
118 | Alpha: 1
119 | Axes Length: 1
120 | Axes Radius: 0.30000001192092896
121 | Color: 255; 25; 0
122 | Head Length: 0.30000001192092896
123 | Head Radius: 0.10000000149011612
124 | Shaft Length: 1
125 | Shaft Radius: 0.05000000074505806
126 | Value: Axes
127 | Topic: /odometry/gps
128 | Unreliable: false
129 | Value: true
130 | - Angle Tolerance: 0.10000000149011612
131 | Class: rviz/Odometry
132 | Covariance:
133 | Orientation:
134 | Alpha: 0.5
135 | Color: 255; 255; 127
136 | Color Style: Unique
137 | Frame: Local
138 | Offset: 1
139 | Scale: 1
140 | Value: true
141 | Position:
142 | Alpha: 0.30000001192092896
143 | Color: 204; 51; 204
144 | Scale: 1
145 | Value: true
146 | Value: false
147 | Enabled: false
148 | Keep: 300
149 | Name: Odom lidar
150 | Position Tolerance: 0.10000000149011612
151 | Shape:
152 | Alpha: 1
153 | Axes Length: 0.25
154 | Axes Radius: 0.10000000149011612
155 | Color: 255; 25; 0
156 | Head Length: 0.30000001192092896
157 | Head Radius: 0.10000000149011612
158 | Shaft Length: 1
159 | Shaft Radius: 0.05000000074505806
160 | Value: Axes
161 | Topic: /lio_sam/mapping/odometry
162 | Unreliable: false
163 | Value: false
164 | Enabled: false
165 | Name: Odometry
166 | - Class: rviz/Group
167 | Displays:
168 | - Alpha: 1
169 | Autocompute Intensity Bounds: true
170 | Autocompute Value Bounds:
171 | Max Value: 10
172 | Min Value: -10
173 | Value: true
174 | Axis: Z
175 | Channel Name: intensity
176 | Class: rviz/PointCloud2
177 | Color: 255; 255; 255
178 | Color Transformer: Intensity
179 | Decay Time: 0
180 | Enabled: true
181 | Invert Rainbow: false
182 | Max Color: 255; 255; 255
183 | Max Intensity: 7188
184 | Min Color: 0; 0; 0
185 | Min Intensity: 3
186 | Name: Velodyne
187 | Position Transformer: XYZ
188 | Queue Size: 10
189 | Selectable: false
190 | Size (Pixels): 2
191 | Size (m): 0.009999999776482582
192 | Style: Points
193 | Topic: /lio_sam/deskew/cloud_deskewed
194 | Unreliable: false
195 | Use Fixed Frame: true
196 | Use rainbow: true
197 | Value: true
198 | - Alpha: 1
199 | Autocompute Intensity Bounds: true
200 | Autocompute Value Bounds:
201 | Max Value: 10
202 | Min Value: -10
203 | Value: true
204 | Axis: Z
205 | Channel Name: intensity
206 | Class: rviz/PointCloud2
207 | Color: 255; 255; 255
208 | Color Transformer: Intensity
209 | Decay Time: 0
210 | Enabled: false
211 | Invert Rainbow: false
212 | Max Color: 255; 255; 255
213 | Max Intensity: 0.7900000214576721
214 | Min Color: 0; 0; 0
215 | Min Intensity: 0
216 | Name: Reg Cloud
217 | Position Transformer: XYZ
218 | Queue Size: 10
219 | Selectable: true
220 | Size (Pixels): 3
221 | Size (m): 0.009999999776482582
222 | Style: Points
223 | Topic: /lio_sam/mapping/cloud_registered
224 | Unreliable: false
225 | Use Fixed Frame: true
226 | Use rainbow: true
227 | Value: false
228 | Enabled: true
229 | Name: Point Cloud
230 | - Class: rviz/Group
231 | Displays:
232 | - Alpha: 1
233 | Autocompute Intensity Bounds: true
234 | Autocompute Value Bounds:
235 | Max Value: 10
236 | Min Value: -10
237 | Value: true
238 | Axis: Z
239 | Channel Name: intensity
240 | Class: rviz/PointCloud2
241 | Color: 0; 255; 0
242 | Color Transformer: FlatColor
243 | Decay Time: 0
244 | Enabled: true
245 | Invert Rainbow: false
246 | Max Color: 255; 255; 255
247 | Max Intensity: 15.077729225158691
248 | Min Color: 0; 0; 0
249 | Min Intensity: 0.019985778257250786
250 | Name: Edge Feature
251 | Position Transformer: XYZ
252 | Queue Size: 10
253 | Selectable: true
254 | Size (Pixels): 5
255 | Size (m): 0.009999999776482582
256 | Style: Points
257 | Topic: /lio_sam/feature/cloud_corner
258 | Unreliable: false
259 | Use Fixed Frame: true
260 | Use rainbow: true
261 | Value: true
262 | - Alpha: 1
263 | Autocompute Intensity Bounds: true
264 | Autocompute Value Bounds:
265 | Max Value: 10
266 | Min Value: -10
267 | Value: true
268 | Axis: Z
269 | Channel Name: intensity
270 | Class: rviz/PointCloud2
271 | Color: 255; 85; 255
272 | Color Transformer: FlatColor
273 | Decay Time: 0
274 | Enabled: true
275 | Invert Rainbow: false
276 | Max Color: 255; 255; 255
277 | Max Intensity: 15.100607872009277
278 | Min Color: 0; 0; 0
279 | Min Intensity: 0.0007188677554950118
280 | Name: Plannar Feature
281 | Position Transformer: XYZ
282 | Queue Size: 10
283 | Selectable: true
284 | Size (Pixels): 3
285 | Size (m): 0.009999999776482582
286 | Style: Points
287 | Topic: /lio_sam/feature/cloud_surface
288 | Unreliable: false
289 | Use Fixed Frame: true
290 | Use rainbow: true
291 | Value: true
292 | Enabled: false
293 | Name: Feature Cloud
294 | - Class: rviz/Group
295 | Displays:
296 | - Alpha: 0.30000001192092896
297 | Autocompute Intensity Bounds: true
298 | Autocompute Value Bounds:
299 | Max Value: 7.4858574867248535
300 | Min Value: -1.2309398651123047
301 | Value: true
302 | Axis: Z
303 | Channel Name: intensity
304 | Class: rviz/PointCloud2
305 | Color: 255; 255; 255
306 | Color Transformer: Intensity
307 | Decay Time: 300
308 | Enabled: false
309 | Invert Rainbow: false
310 | Max Color: 255; 255; 255
311 | Max Intensity: 2569
312 | Min Color: 0; 0; 0
313 | Min Intensity: 10
314 | Name: Map (cloud)
315 | Position Transformer: XYZ
316 | Queue Size: 10
317 | Selectable: false
318 | Size (Pixels): 2
319 | Size (m): 0.009999999776482582
320 | Style: Points
321 | Topic: /lio_sam/mapping/cloud_registered
322 | Unreliable: false
323 | Use Fixed Frame: true
324 | Use rainbow: true
325 | Value: false
326 | - Alpha: 1
327 | Autocompute Intensity Bounds: true
328 | Autocompute Value Bounds:
329 | Max Value: 36.61034393310547
330 | Min Value: -2.3476977348327637
331 | Value: true
332 | Axis: Z
333 | Channel Name: intensity
334 | Class: rviz/PointCloud2
335 | Color: 255; 255; 255
336 | Color Transformer: AxisColor
337 | Decay Time: 0
338 | Enabled: false
339 | Invert Rainbow: false
340 | Max Color: 255; 255; 255
341 | Max Intensity: 15.100604057312012
342 | Min Color: 0; 0; 0
343 | Min Intensity: 0.014545874670147896
344 | Name: Map (local)
345 | Position Transformer: XYZ
346 | Queue Size: 10
347 | Selectable: true
348 | Size (Pixels): 2
349 | Size (m): 0.10000000149011612
350 | Style: Flat Squares
351 | Topic: /lio_sam/mapping/map_local
352 | Unreliable: false
353 | Use Fixed Frame: true
354 | Use rainbow: true
355 | Value: false
356 | - Alpha: 1
357 | Autocompute Intensity Bounds: false
358 | Autocompute Value Bounds:
359 | Max Value: 10
360 | Min Value: -10
361 | Value: true
362 | Axis: Z
363 | Channel Name: intensity
364 | Class: rviz/PointCloud2
365 | Color: 255; 255; 255
366 | Color Transformer: Intensity
367 | Decay Time: 0
368 | Enabled: true
369 | Invert Rainbow: false
370 | Max Color: 255; 255; 255
371 | Max Intensity: 300
372 | Min Color: 0; 0; 0
373 | Min Intensity: 10
374 | Name: Map (global)
375 | Position Transformer: XYZ
376 | Queue Size: 10
377 | Selectable: true
378 | Size (Pixels): 2
379 | Size (m): 0.009999999776482582
380 | Style: Points
381 | Topic: /lio_sam/mapping/map_global
382 | Unreliable: false
383 | Use Fixed Frame: true
384 | Use rainbow: true
385 | Value: true
386 | - Alpha: 1
387 | Buffer Length: 1
388 | Class: rviz/Path
389 | Color: 238; 238; 236
390 | Enabled: true
391 | Head Diameter: 0.30000001192092896
392 | Head Length: 0.20000000298023224
393 | Length: 0.30000001192092896
394 | Line Style: Billboards
395 | Line Width: 1
396 | Name: Path (global)
397 | Offset:
398 | X: 0
399 | Y: 0
400 | Z: 0
401 | Pose Color: 255; 85; 255
402 | Pose Style: None
403 | Radius: 0.029999999329447746
404 | Shaft Diameter: 0.10000000149011612
405 | Shaft Length: 0.10000000149011612
406 | Topic: /lio_sam/mapping/path
407 | Unreliable: false
408 | Value: true
409 | - Alpha: 1
410 | Buffer Length: 1
411 | Class: rviz/Path
412 | Color: 255; 85; 255
413 | Enabled: true
414 | Head Diameter: 0.30000001192092896
415 | Head Length: 0.20000000298023224
416 | Length: 0.30000001192092896
417 | Line Style: Billboards
418 | Line Width: 0.10000000149011612
419 | Name: Path (local)
420 | Offset:
421 | X: 0
422 | Y: 0
423 | Z: 0
424 | Pose Color: 255; 85; 255
425 | Pose Style: None
426 | Radius: 0.029999999329447746
427 | Shaft Diameter: 0.10000000149011612
428 | Shaft Length: 0.10000000149011612
429 | Topic: /lio_sam/imu/path
430 | Unreliable: false
431 | Value: true
432 | - Alpha: 1
433 | Autocompute Intensity Bounds: true
434 | Autocompute Value Bounds:
435 | Max Value: 20.802837371826172
436 | Min Value: -0.03934507071971893
437 | Value: true
438 | Axis: Z
439 | Channel Name: intensity
440 | Class: rviz/PointCloud2
441 | Color: 255; 255; 255
442 | Color Transformer: AxisColor
443 | Decay Time: 0
444 | Enabled: false
445 | Invert Rainbow: false
446 | Max Color: 255; 255; 255
447 | Max Intensity: 15.100604057312012
448 | Min Color: 0; 0; 0
449 | Min Intensity: 0.014545874670147896
450 | Name: Loop closure
451 | Position Transformer: XYZ
452 | Queue Size: 10
453 | Selectable: true
454 | Size (Pixels): 2
455 | Size (m): 0.30000001192092896
456 | Style: Flat Squares
457 | Topic: /lio_sam/mapping/icp_loop_closure_corrected_cloud
458 | Unreliable: false
459 | Use Fixed Frame: true
460 | Use rainbow: true
461 | Value: false
462 | - Class: rviz/MarkerArray
463 | Enabled: true
464 | Marker Topic: /lio_sam/mapping/loop_closure_constraints
465 | Name: Loop constraint
466 | Namespaces:
467 | {}
468 | Queue Size: 100
469 | Value: true
470 | Enabled: true
471 | Name: Mapping
472 | Enabled: true
473 | Global Options:
474 | Background Color: 0; 0; 0
475 | Default Light: true
476 | Fixed Frame: map
477 | Frame Rate: 30
478 | Name: root
479 | Tools:
480 | - Class: rviz/Interact
481 | Hide Inactive Objects: true
482 | - Class: rviz/FocusCamera
483 | - Class: rviz/Measure
484 | Value: true
485 | Views:
486 | Current:
487 | Class: rviz/Orbit
488 | Distance: 143.16458129882812
489 | Enable Stereo Rendering:
490 | Stereo Eye Separation: 0.05999999865889549
491 | Stereo Focal Distance: 1
492 | Swap Stereo Eyes: false
493 | Value: false
494 | Focal Point:
495 | X: 1.5083352327346802
496 | Y: 2.730443000793457
497 | Z: -6.330533981323242
498 | Focal Shape Fixed Size: false
499 | Focal Shape Size: 0.05000000074505806
500 | Invert Z Axis: false
501 | Name: Current View
502 | Near Clip Distance: 0.009999999776482582
503 | Pitch: 0.7847971320152283
504 | Target Frame: base_link
505 | Value: Orbit (rviz)
506 | Yaw: 5.183582782745361
507 | Saved: ~
508 | Window Geometry:
509 | Displays:
510 | collapsed: false
511 | Height: 1043
512 | Hide Left Dock: false
513 | Hide Right Dock: false
514 | QMainWindow State: 000000ff00000000fd00000004000000000000015b000003b9fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000002e000001340000000000000000fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000286000000a70000005c00ffffff0000000100000111000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000040fc0100000002fb0000000800540069006d00650000000000000004b00000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000508000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
515 | Selection:
516 | collapsed: false
517 | Tool Properties:
518 | collapsed: false
519 | Views:
520 | collapsed: false
521 | Width: 1920
522 | X: 3840
523 | Y: 0
524 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/module_loam.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/module_navsat.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
9 |
11 |
12 |
13 |
14 |
15 |
19 |
20 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/module_robot_state_publisher.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
8 |
9 |
10 |
11 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/module_rviz.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
9 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/rosconsole/rosconsole_error.conf:
--------------------------------------------------------------------------------
1 | # Set the default ros output to warning and higher
2 | log4j.logger.ros = ERROR
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/rosconsole/rosconsole_info.conf:
--------------------------------------------------------------------------------
1 | # Set the default ros output to warning and higher
2 | log4j.logger.ros = INFO
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/include/rosconsole/rosconsole_warn.conf:
--------------------------------------------------------------------------------
1 | # Set the default ros output to warning and higher
2 | log4j.logger.ros = WARN
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/run.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
21 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/launch/run_mulran.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
21 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/msg/cloud_info.msg:
--------------------------------------------------------------------------------
1 | # Cloud Info
2 | Header header
3 |
4 | int32[] startRingIndex
5 | int32[] endRingIndex
6 |
7 | int32[] pointColInd # point column index in range image
8 | float32[] pointRange # point range
9 |
10 | int64 imuAvailable
11 | int64 odomAvailable
12 |
13 | # Attitude for LOAM initialization
14 | float32 imuRollInit
15 | float32 imuPitchInit
16 | float32 imuYawInit
17 |
18 | # Initial guess from imu pre-integration
19 | float32 initialGuessX
20 | float32 initialGuessY
21 | float32 initialGuessZ
22 | float32 initialGuessRoll
23 | float32 initialGuessPitch
24 | float32 initialGuessYaw
25 |
26 | # Point cloud messages
27 | sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed
28 | sensor_msgs/PointCloud2 cloud_corner # extracted corner feature
29 | sensor_msgs/PointCloud2 cloud_surface # extracted surface feature
--------------------------------------------------------------------------------
/SC-LIO-SAM/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | lio_sam
4 | 1.0.0
5 | Lidar Odometry
6 |
7 | Tixiao Shan
8 |
9 | TODO
10 |
11 | Tixiao Shan
12 |
13 | catkin
14 |
15 | roscpp
16 | roscpp
17 | rospy
18 | rospy
19 |
20 | tf
21 | tf
22 |
23 | cv_bridge
24 | cv_bridge
25 |
26 | pcl_conversions
27 | pcl_conversions
28 |
29 | std_msgs
30 | std_msgs
31 | sensor_msgs
32 | sensor_msgs
33 | geometry_msgs
34 | geometry_msgs
35 | nav_msgs
36 | nav_msgs
37 |
38 | message_generation
39 | message_generation
40 | message_runtime
41 | message_runtime
42 |
43 | GTSAM
44 | GTSAM
45 |
46 |
47 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/src/Scancontext.cpp:
--------------------------------------------------------------------------------
1 | #include "Scancontext.h"
2 |
3 | // namespace SC2
4 | // {
5 |
6 | void coreImportTest (void)
7 | {
8 | cout << "scancontext lib is successfully imported." << endl;
9 | } // coreImportTest
10 |
11 |
12 | float rad2deg(float radians)
13 | {
14 | return radians * 180.0 / M_PI;
15 | }
16 |
17 | float deg2rad(float degrees)
18 | {
19 | return degrees * M_PI / 180.0;
20 | }
21 |
22 |
23 | float xy2theta( const float & _x, const float & _y )
24 | {
25 | if ( (_x >= 0) & (_y >= 0))
26 | return (180/M_PI) * atan(_y / _x);
27 |
28 | if ( (_x < 0) & (_y >= 0))
29 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) );
30 |
31 | if ( (_x < 0) & (_y < 0))
32 | return 180 + ( (180/M_PI) * atan(_y / _x) );
33 |
34 | if ( (_x >= 0) & (_y < 0))
35 | return 360 - ( (180/M_PI) * atan((-_y) / _x) );
36 | } // xy2theta
37 |
38 |
39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift )
40 | {
41 | // shift columns to right direction
42 | assert(_num_shift >= 0);
43 |
44 | if( _num_shift == 0 )
45 | {
46 | MatrixXd shifted_mat( _mat );
47 | return shifted_mat; // Early return
48 | }
49 |
50 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() );
51 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
52 | {
53 | int new_location = (col_idx + _num_shift) % _mat.cols();
54 | shifted_mat.col(new_location) = _mat.col(col_idx);
55 | }
56 |
57 | return shifted_mat;
58 |
59 | } // circshift
60 |
61 |
62 | std::vector eig2stdvec( MatrixXd _eigmat )
63 | {
64 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
65 | return vec;
66 | } // eig2stdvec
67 |
68 |
69 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 )
70 | {
71 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
72 | double sum_sector_similarity = 0;
73 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
74 | {
75 | VectorXd col_sc1 = _sc1.col(col_idx);
76 | VectorXd col_sc2 = _sc2.col(col_idx);
77 |
78 | if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) )
79 | continue; // don't count this sector pair.
80 |
81 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());
82 |
83 | sum_sector_similarity = sum_sector_similarity + sector_similarity;
84 | num_eff_cols = num_eff_cols + 1;
85 | }
86 |
87 | double sc_sim = sum_sector_similarity / num_eff_cols;
88 | return 1.0 - sc_sim;
89 |
90 | } // distDirectSC
91 |
92 |
93 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
94 | {
95 | int argmin_vkey_shift = 0;
96 | double min_veky_diff_norm = 10000000;
97 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
98 | {
99 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
100 |
101 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
102 |
103 | double cur_diff_norm = vkey_diff.norm();
104 | if( cur_diff_norm < min_veky_diff_norm )
105 | {
106 | argmin_vkey_shift = shift_idx;
107 | min_veky_diff_norm = cur_diff_norm;
108 | }
109 | }
110 |
111 | return argmin_vkey_shift;
112 |
113 | } // fastAlignUsingVkey
114 |
115 |
116 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
117 | {
118 | // 1. fast align using variant key (not in original IROS18)
119 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
120 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
121 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
122 |
123 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range
124 | std::vector shift_idx_search_space { argmin_vkey_shift };
125 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
126 | {
127 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
128 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
129 | }
130 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
131 |
132 | // 2. fast columnwise diff
133 | int argmin_shift = 0;
134 | double min_sc_dist = 10000000;
135 | for ( int num_shift: shift_idx_search_space )
136 | {
137 | MatrixXd sc2_shifted = circshift(_sc2, num_shift);
138 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
139 | if( cur_sc_dist < min_sc_dist )
140 | {
141 | argmin_shift = num_shift;
142 | min_sc_dist = cur_sc_dist;
143 | }
144 | }
145 |
146 | return make_pair(min_sc_dist, argmin_shift);
147 |
148 | } // distanceBtnScanContext
149 |
150 |
151 | MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down )
152 | {
153 | TicToc t_making_desc;
154 |
155 | int num_pts_scan_down = _scan_down.points.size();
156 |
157 | // main
158 | const int NO_POINT = -1000;
159 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
160 |
161 | SCPointType pt;
162 | float azim_angle, azim_range; // wihtin 2d plane
163 | int ring_idx, sctor_idx;
164 | for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
165 | {
166 | pt.x = _scan_down.points[pt_idx].x;
167 | pt.y = _scan_down.points[pt_idx].y;
168 | pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
169 |
170 | // xyz to ring, sector
171 | azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
172 | azim_angle = xy2theta(pt.x, pt.y);
173 |
174 | // if range is out of roi, pass
175 | if( azim_range > PC_MAX_RADIUS )
176 | continue;
177 |
178 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
179 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );
180 |
181 | // taking maximum z
182 | if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
183 | desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
184 | }
185 |
186 | // reset no points to zero (for cosine dist later)
187 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
188 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
189 | if( desc(row_idx, col_idx) == NO_POINT )
190 | desc(row_idx, col_idx) = 0;
191 |
192 | t_making_desc.toc("PolarContext making");
193 |
194 | return desc;
195 | } // SCManager::makeScancontext
196 |
197 |
198 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
199 | {
200 | /*
201 | * summary: rowwise mean vector
202 | */
203 | Eigen::MatrixXd invariant_key(_desc.rows(), 1);
204 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
205 | {
206 | Eigen::MatrixXd curr_row = _desc.row(row_idx);
207 | invariant_key(row_idx, 0) = curr_row.mean();
208 | }
209 |
210 | return invariant_key;
211 | } // SCManager::makeRingkeyFromScancontext
212 |
213 |
214 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
215 | {
216 | /*
217 | * summary: columnwise mean vector
218 | */
219 | Eigen::MatrixXd variant_key(1, _desc.cols());
220 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
221 | {
222 | Eigen::MatrixXd curr_col = _desc.col(col_idx);
223 | variant_key(0, col_idx) = curr_col.mean();
224 | }
225 |
226 | return variant_key;
227 | } // SCManager::makeSectorkeyFromScancontext
228 |
229 |
230 | const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void)
231 | {
232 | return polarcontexts_.back();
233 | }
234 |
235 |
236 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down )
237 | {
238 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
239 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
240 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
241 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey );
242 |
243 | polarcontexts_.push_back( sc );
244 | polarcontext_invkeys_.push_back( ringkey );
245 | polarcontext_vkeys_.push_back( sectorkey );
246 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
247 |
248 | // cout < SCManager::detectLoopClosureID ( void )
254 | {
255 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
256 |
257 | auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
258 | auto curr_desc = polarcontexts_.back(); // current observation (query)
259 |
260 | /*
261 | * step 1: candidates from ringkey tree_
262 | */
263 | if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)
264 | {
265 | std::pair result {loop_id, 0.0};
266 | return result; // Early return
267 | }
268 |
269 | // tree_ reconstruction (not mandatory to make everytime)
270 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
271 | {
272 | TicToc t_tree_construction;
273 |
274 | polarcontext_invkeys_to_search_.clear();
275 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
276 |
277 | polarcontext_tree_.reset();
278 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
279 | // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
280 | t_tree_construction.toc("Tree construction");
281 | }
282 | tree_making_period_conter = tree_making_period_conter + 1;
283 |
284 | double min_dist = 10000000; // init with somthing large
285 | int nn_align = 0;
286 | int nn_idx = 0;
287 |
288 | // knn search
289 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE );
290 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
291 |
292 | TicToc t_tree_search;
293 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE );
294 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
295 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) );
296 | t_tree_search.toc("Tree search");
297 |
298 | /*
299 | * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
300 | */
301 | TicToc t_calc_dist;
302 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
303 | {
304 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
305 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate );
306 |
307 | double candidate_dist = sc_dist_result.first;
308 | int candidate_align = sc_dist_result.second;
309 |
310 | if( candidate_dist < min_dist )
311 | {
312 | min_dist = candidate_dist;
313 | nn_align = candidate_align;
314 |
315 | nn_idx = candidate_indexes[candidate_iter_idx];
316 | }
317 | }
318 | t_calc_dist.toc("Distance calc");
319 |
320 | /*
321 | * loop threshold check
322 | */
323 | if( min_dist < SC_DIST_THRES )
324 | {
325 | loop_id = nn_idx;
326 |
327 | // std::cout.precision(3);
328 | cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
329 | cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
330 | }
331 | else
332 | {
333 | std::cout.precision(3);
334 | cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
335 | cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
336 | }
337 |
338 | // To do: return also nn_align (i.e., yaw diff)
339 | float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
340 | std::pair result {loop_id, yaw_diff_rad};
341 |
342 | return result;
343 |
344 | } // SCManager::detectLoopClosureID
345 |
346 | // } // namespace SC2
--------------------------------------------------------------------------------
/SC-LIO-SAM/src/featureExtraction.cpp:
--------------------------------------------------------------------------------
1 | #include "utility.h"
2 | #include "lio_sam/cloud_info.h"
3 |
4 | struct smoothness_t{
5 | float value;
6 | size_t ind;
7 | };
8 |
9 | struct by_value{
10 | bool operator()(smoothness_t const &left, smoothness_t const &right) {
11 | return left.value < right.value;
12 | }
13 | };
14 |
15 | class FeatureExtraction : public ParamServer
16 | {
17 |
18 | public:
19 |
20 | ros::Subscriber subLaserCloudInfo;
21 |
22 | ros::Publisher pubLaserCloudInfo;
23 | ros::Publisher pubCornerPoints;
24 | ros::Publisher pubSurfacePoints;
25 |
26 | pcl::PointCloud::Ptr extractedCloud;
27 | pcl::PointCloud::Ptr cornerCloud;
28 | pcl::PointCloud::Ptr surfaceCloud;
29 |
30 | pcl::VoxelGrid downSizeFilter;
31 |
32 | lio_sam::cloud_info cloudInfo;
33 | std_msgs::Header cloudHeader;
34 |
35 | std::vector cloudSmoothness;
36 | float *cloudCurvature;
37 | int *cloudNeighborPicked;
38 | int *cloudLabel;
39 |
40 | FeatureExtraction()
41 | {
42 | subLaserCloudInfo = nh.subscribe("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
43 |
44 | pubLaserCloudInfo = nh.advertise ("lio_sam/feature/cloud_info", 1);
45 | pubCornerPoints = nh.advertise("lio_sam/feature/cloud_corner", 1);
46 | pubSurfacePoints = nh.advertise("lio_sam/feature/cloud_surface", 1);
47 |
48 | initializationValue();
49 | }
50 |
51 | void initializationValue()
52 | {
53 | cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
54 |
55 | downSizeFilter.setLeafSize(odometrySurfLeafSize, odometrySurfLeafSize, odometrySurfLeafSize);
56 |
57 | extractedCloud.reset(new pcl::PointCloud());
58 | cornerCloud.reset(new pcl::PointCloud());
59 | surfaceCloud.reset(new pcl::PointCloud());
60 |
61 | cloudCurvature = new float[N_SCAN*Horizon_SCAN];
62 | cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
63 | cloudLabel = new int[N_SCAN*Horizon_SCAN];
64 | }
65 |
66 | void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
67 | {
68 | cloudInfo = *msgIn; // new cloud info
69 | cloudHeader = msgIn->header; // new cloud header
70 | pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
71 |
72 | calculateSmoothness();
73 |
74 | markOccludedPoints();
75 |
76 | extractFeatures();
77 |
78 | publishFeatureCloud();
79 | }
80 |
81 | void calculateSmoothness()
82 | {
83 | int cloudSize = extractedCloud->points.size();
84 | for (int i = 5; i < cloudSize - 5; i++)
85 | {
86 | float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
87 | + cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
88 | + cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
89 | + cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
90 | + cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
91 | + cloudInfo.pointRange[i+5];
92 |
93 | cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
94 |
95 | cloudNeighborPicked[i] = 0;
96 | cloudLabel[i] = 0;
97 | // cloudSmoothness for sorting
98 | cloudSmoothness[i].value = cloudCurvature[i];
99 | cloudSmoothness[i].ind = i;
100 | }
101 | }
102 |
103 | void markOccludedPoints()
104 | {
105 | int cloudSize = extractedCloud->points.size();
106 | // mark occluded points and parallel beam points
107 | for (int i = 5; i < cloudSize - 6; ++i)
108 | {
109 | // occluded points
110 | float depth1 = cloudInfo.pointRange[i];
111 | float depth2 = cloudInfo.pointRange[i+1];
112 | int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
113 |
114 | if (columnDiff < 10){
115 | // 10 pixel diff in range image
116 | if (depth1 - depth2 > 0.3){
117 | cloudNeighborPicked[i - 5] = 1;
118 | cloudNeighborPicked[i - 4] = 1;
119 | cloudNeighborPicked[i - 3] = 1;
120 | cloudNeighborPicked[i - 2] = 1;
121 | cloudNeighborPicked[i - 1] = 1;
122 | cloudNeighborPicked[i] = 1;
123 | }else if (depth2 - depth1 > 0.3){
124 | cloudNeighborPicked[i + 1] = 1;
125 | cloudNeighborPicked[i + 2] = 1;
126 | cloudNeighborPicked[i + 3] = 1;
127 | cloudNeighborPicked[i + 4] = 1;
128 | cloudNeighborPicked[i + 5] = 1;
129 | cloudNeighborPicked[i + 6] = 1;
130 | }
131 | }
132 | // parallel beam
133 | float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
134 | float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
135 |
136 | if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
137 | cloudNeighborPicked[i] = 1;
138 | }
139 | }
140 |
141 | void extractFeatures()
142 | {
143 | cornerCloud->clear();
144 | surfaceCloud->clear();
145 |
146 | pcl::PointCloud::Ptr surfaceCloudScan(new pcl::PointCloud());
147 | pcl::PointCloud::Ptr surfaceCloudScanDS(new pcl::PointCloud());
148 |
149 | for (int i = 0; i < N_SCAN; i++)
150 | {
151 | surfaceCloudScan->clear();
152 |
153 | for (int j = 0; j < 6; j++)
154 | {
155 |
156 | int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
157 | int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
158 |
159 | if (sp >= ep)
160 | continue;
161 |
162 | std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
163 |
164 | int largestPickedNum = 0;
165 | for (int k = ep; k >= sp; k--)
166 | {
167 | int ind = cloudSmoothness[k].ind;
168 | if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
169 | {
170 | largestPickedNum++;
171 | if (largestPickedNum <= 20){
172 | cloudLabel[ind] = 1;
173 | cornerCloud->push_back(extractedCloud->points[ind]);
174 | } else {
175 | break;
176 | }
177 |
178 | cloudNeighborPicked[ind] = 1;
179 | for (int l = 1; l <= 5; l++)
180 | {
181 | int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
182 | if (columnDiff > 10)
183 | break;
184 | cloudNeighborPicked[ind + l] = 1;
185 | }
186 | for (int l = -1; l >= -5; l--)
187 | {
188 | int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
189 | if (columnDiff > 10)
190 | break;
191 | cloudNeighborPicked[ind + l] = 1;
192 | }
193 | }
194 | }
195 |
196 | for (int k = sp; k <= ep; k++)
197 | {
198 | int ind = cloudSmoothness[k].ind;
199 | if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
200 | {
201 |
202 | cloudLabel[ind] = -1;
203 | cloudNeighborPicked[ind] = 1;
204 |
205 | for (int l = 1; l <= 5; l++) {
206 |
207 | int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
208 | if (columnDiff > 10)
209 | break;
210 |
211 | cloudNeighborPicked[ind + l] = 1;
212 | }
213 | for (int l = -1; l >= -5; l--) {
214 |
215 | int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
216 | if (columnDiff > 10)
217 | break;
218 |
219 | cloudNeighborPicked[ind + l] = 1;
220 | }
221 | }
222 | }
223 |
224 | for (int k = sp; k <= ep; k++)
225 | {
226 | if (cloudLabel[k] <= 0){
227 | surfaceCloudScan->push_back(extractedCloud->points[k]);
228 | }
229 | }
230 | }
231 |
232 | surfaceCloudScanDS->clear();
233 | downSizeFilter.setInputCloud(surfaceCloudScan);
234 | downSizeFilter.filter(*surfaceCloudScanDS);
235 |
236 | *surfaceCloud += *surfaceCloudScanDS;
237 | }
238 | }
239 |
240 | void freeCloudInfoMemory()
241 | {
242 | cloudInfo.startRingIndex.clear();
243 | cloudInfo.endRingIndex.clear();
244 | cloudInfo.pointColInd.clear();
245 | cloudInfo.pointRange.clear();
246 | }
247 |
248 | void publishFeatureCloud()
249 | {
250 | // free cloud info memory
251 | freeCloudInfoMemory();
252 | // save newly extracted features
253 | cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
254 | cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
255 | // publish to mapOptimization
256 | pubLaserCloudInfo.publish(cloudInfo);
257 | }
258 | };
259 |
260 |
261 | int main(int argc, char** argv)
262 | {
263 | ros::init(argc, argv, "lio_sam");
264 |
265 | FeatureExtraction FE;
266 |
267 | ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
268 |
269 | ros::spin();
270 |
271 | return 0;
272 | }
--------------------------------------------------------------------------------
/SC-LIO-SAM/src/imageProjection.cpp:
--------------------------------------------------------------------------------
1 | #include "utility.h"
2 | #include "lio_sam/cloud_info.h"
3 |
4 | struct VelodynePointXYZIRT
5 | {
6 | PCL_ADD_POINT4D
7 | PCL_ADD_INTENSITY;
8 | uint16_t ring;
9 | float time;
10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
11 | } EIGEN_ALIGN16;
12 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
13 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
14 | (uint16_t, ring, ring) (float, time, time)
15 | )
16 |
17 | struct OusterPointXYZIRT {
18 | PCL_ADD_POINT4D;
19 | float intensity;
20 | uint32_t t;
21 | uint16_t reflectivity;
22 | uint8_t ring;
23 | uint16_t noise;
24 | uint32_t range;
25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
26 | } EIGEN_ALIGN16;
27 | POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
28 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
29 | (uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
30 | (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
31 | )
32 |
33 | struct MulranPointXYZIRT { // from the file player's topic https://github.com/irapkaist/file_player_mulran, see https://github.com/irapkaist/file_player_mulran/blob/17da0cb6ef66b4971ec943ab8d234aa25da33e7e/src/ROSThread.cpp#L7
34 | PCL_ADD_POINT4D;
35 | float intensity;
36 | uint32_t t;
37 | int ring;
38 |
39 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW
40 | }EIGEN_ALIGN16;
41 | POINT_CLOUD_REGISTER_POINT_STRUCT (MulranPointXYZIRT,
42 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
43 | (uint32_t, t, t) (int, ring, ring)
44 | )
45 |
46 | // Use the Velodyne point format as a common representation
47 | using PointXYZIRT = VelodynePointXYZIRT;
48 |
49 | const int queueLength = 2000;
50 |
51 | class ImageProjection : public ParamServer
52 | {
53 | private:
54 |
55 | std::mutex imuLock;
56 | std::mutex odoLock;
57 |
58 | ros::Subscriber subLaserCloud;
59 | ros::Publisher pubLaserCloud;
60 |
61 | ros::Publisher pubExtractedCloud;
62 | ros::Publisher pubLaserCloudInfo;
63 |
64 | ros::Subscriber subImu;
65 | std::deque imuQueue;
66 |
67 | ros::Subscriber subOdom;
68 | std::deque odomQueue;
69 |
70 | std::deque cloudQueue;
71 | sensor_msgs::PointCloud2 currentCloudMsg;
72 |
73 | double *imuTime = new double[queueLength];
74 | double *imuRotX = new double[queueLength];
75 | double *imuRotY = new double[queueLength];
76 | double *imuRotZ = new double[queueLength];
77 |
78 | int imuPointerCur;
79 | bool firstPointFlag;
80 | Eigen::Affine3f transStartInverse;
81 |
82 | pcl::PointCloud::Ptr laserCloudIn;
83 | pcl::PointCloud::Ptr tmpOusterCloudIn;
84 | pcl::PointCloud::Ptr tmpMulranCloudIn;
85 | pcl::PointCloud::Ptr fullCloud;
86 | pcl::PointCloud::Ptr extractedCloud;
87 |
88 | int deskewFlag;
89 | cv::Mat rangeMat;
90 |
91 | bool odomDeskewFlag;
92 | float odomIncreX;
93 | float odomIncreY;
94 | float odomIncreZ;
95 |
96 | lio_sam::cloud_info cloudInfo;
97 | double timeScanCur;
98 | double timeScanEnd;
99 | std_msgs::Header cloudHeader;
100 |
101 |
102 | public:
103 | ImageProjection():
104 | deskewFlag(0)
105 | {
106 | subImu = nh.subscribe(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
107 | subOdom = nh.subscribe(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
108 | subLaserCloud = nh.subscribe(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
109 |
110 | pubExtractedCloud = nh.advertise ("lio_sam/deskew/cloud_deskewed", 1);
111 | pubLaserCloudInfo = nh.advertise ("lio_sam/deskew/cloud_info", 1);
112 |
113 | allocateMemory();
114 | resetParameters();
115 |
116 | pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
117 | }
118 |
119 | void allocateMemory()
120 | {
121 | laserCloudIn.reset(new pcl::PointCloud());
122 | tmpOusterCloudIn.reset(new pcl::PointCloud());
123 | tmpMulranCloudIn.reset(new pcl::PointCloud());
124 | fullCloud.reset(new pcl::PointCloud());
125 | extractedCloud.reset(new pcl::PointCloud());
126 |
127 | fullCloud->points.resize(N_SCAN*Horizon_SCAN);
128 |
129 | cloudInfo.startRingIndex.assign(N_SCAN, 0);
130 | cloudInfo.endRingIndex.assign(N_SCAN, 0);
131 |
132 | cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
133 | cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);
134 |
135 | resetParameters();
136 | }
137 |
138 | void resetParameters()
139 | {
140 | laserCloudIn->clear();
141 | extractedCloud->clear();
142 | // reset range matrix for range image projection
143 | rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
144 |
145 | imuPointerCur = 0;
146 | firstPointFlag = true;
147 | odomDeskewFlag = false;
148 |
149 | for (int i = 0; i < queueLength; ++i)
150 | {
151 | imuTime[i] = 0;
152 | imuRotX[i] = 0;
153 | imuRotY[i] = 0;
154 | imuRotZ[i] = 0;
155 | }
156 | }
157 |
158 | ~ImageProjection(){}
159 |
160 | void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
161 | {
162 | sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
163 |
164 | std::lock_guard lock1(imuLock);
165 | imuQueue.push_back(thisImu);
166 |
167 | // debug IMU data
168 | // cout << std::setprecision(6);
169 | // cout << "IMU acc: " << endl;
170 | // cout << "x: " << thisImu.linear_acceleration.x <<
171 | // ", y: " << thisImu.linear_acceleration.y <<
172 | // ", z: " << thisImu.linear_acceleration.z << endl;
173 | // cout << "IMU gyro: " << endl;
174 | // cout << "x: " << thisImu.angular_velocity.x <<
175 | // ", y: " << thisImu.angular_velocity.y <<
176 | // ", z: " << thisImu.angular_velocity.z << endl;
177 | // double imuRoll, imuPitch, imuYaw;
178 | // tf::Quaternion orientation;
179 | // tf::quaternionMsgToTF(thisImu.orientation, orientation);
180 | // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
181 | // cout << "IMU roll pitch yaw: " << endl;
182 | // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
183 | }
184 |
185 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
186 | {
187 | std::lock_guard lock2(odoLock);
188 | odomQueue.push_back(*odometryMsg);
189 | }
190 |
191 | void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
192 | {
193 | if (!cachePointCloud(laserCloudMsg))
194 | return;
195 |
196 | if (!deskewInfo())
197 | return;
198 |
199 | projectPointCloud();
200 |
201 | cloudExtraction();
202 |
203 | publishClouds();
204 |
205 | resetParameters();
206 | }
207 |
208 | bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
209 | {
210 | // cache point cloud
211 | cloudQueue.push_back(*laserCloudMsg);
212 | if (cloudQueue.size() <= 2)
213 | return false;
214 |
215 | // convert cloud
216 | currentCloudMsg = std::move(cloudQueue.front());
217 |
218 | cloudHeader = currentCloudMsg.header;
219 | timeScanCur = cloudHeader.stamp.toSec();
220 |
221 | cloudQueue.pop_front();
222 |
223 | if (sensor == SensorType::VELODYNE)
224 | {
225 | pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
226 | }
227 | else if (sensor == SensorType::OUSTER)
228 | {
229 | // Convert to Velodyne format
230 | pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
231 | laserCloudIn->points.resize(tmpOusterCloudIn->size());
232 | laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
233 | for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
234 | {
235 | auto &src = tmpOusterCloudIn->points[i];
236 | auto &dst = laserCloudIn->points[i];
237 | dst.x = src.x;
238 | dst.y = src.y;
239 | dst.z = src.z;
240 | dst.intensity = src.intensity;
241 | dst.ring = src.ring;
242 | dst.time = src.t * 1e-9f;
243 | }
244 | }
245 | else if (sensor == SensorType::MULRAN)
246 | {
247 | // Convert to Velodyne format
248 | pcl::moveFromROSMsg(currentCloudMsg, *tmpMulranCloudIn);
249 | laserCloudIn->points.resize(tmpMulranCloudIn->size());
250 | laserCloudIn->is_dense = tmpMulranCloudIn->is_dense;
251 | for (size_t i = 0; i < tmpMulranCloudIn->size(); i++)
252 | {
253 | auto &src = tmpMulranCloudIn->points[i];
254 | auto &dst = laserCloudIn->points[i];
255 | dst.x = src.x;
256 | dst.y = src.y;
257 | dst.z = src.z;
258 | dst.intensity = src.intensity;
259 | dst.ring = src.ring;
260 | dst.time = float(src.t);
261 | }
262 | }
263 | else
264 | {
265 | ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
266 | ros::shutdown();
267 | }
268 |
269 | // get timestamp
270 | timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
271 |
272 | // check dense flag
273 | if (laserCloudIn->is_dense == false)
274 | {
275 | ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
276 | ros::shutdown();
277 | }
278 |
279 | // check ring channel
280 | static int ringFlag = 0;
281 | if (ringFlag == 0)
282 | {
283 | ringFlag = -1;
284 | for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
285 | {
286 | if (currentCloudMsg.fields[i].name == "ring")
287 | {
288 | ringFlag = 1;
289 | break;
290 | }
291 | }
292 | if (ringFlag == -1)
293 | {
294 | ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
295 | ros::shutdown();
296 | }
297 | }
298 |
299 | // check point time
300 | if (deskewFlag == 0)
301 | {
302 | deskewFlag = -1;
303 | for (auto &field : currentCloudMsg.fields)
304 | {
305 | if (field.name == "time" || field.name == "t")
306 | {
307 | deskewFlag = 1;
308 | break;
309 | }
310 | }
311 | if (deskewFlag == -1)
312 | ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
313 | }
314 |
315 | return true;
316 | }
317 |
318 | bool deskewInfo()
319 | {
320 | std::lock_guard lock1(imuLock);
321 | std::lock_guard lock2(odoLock);
322 |
323 | // make sure IMU data available for the scan
324 | if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
325 | {
326 | ROS_DEBUG("Waiting for IMU data ...");
327 | return false;
328 | }
329 |
330 | imuDeskewInfo();
331 |
332 | odomDeskewInfo();
333 |
334 | return true;
335 | }
336 |
337 | void imuDeskewInfo()
338 | {
339 | cloudInfo.imuAvailable = false;
340 |
341 | while (!imuQueue.empty())
342 | {
343 | if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
344 | imuQueue.pop_front();
345 | else
346 | break;
347 | }
348 |
349 | if (imuQueue.empty())
350 | return;
351 |
352 | imuPointerCur = 0;
353 |
354 | for (int i = 0; i < (int)imuQueue.size(); ++i)
355 | {
356 | sensor_msgs::Imu thisImuMsg = imuQueue[i];
357 | double currentImuTime = thisImuMsg.header.stamp.toSec();
358 |
359 | // get roll, pitch, and yaw estimation for this scan
360 | if (currentImuTime <= timeScanCur)
361 | imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
362 |
363 | if (currentImuTime > timeScanEnd + 0.01)
364 | break;
365 |
366 | if (imuPointerCur == 0){
367 | imuRotX[0] = 0;
368 | imuRotY[0] = 0;
369 | imuRotZ[0] = 0;
370 | imuTime[0] = currentImuTime;
371 | ++imuPointerCur;
372 | continue;
373 | }
374 |
375 | // get angular velocity
376 | double angular_x, angular_y, angular_z;
377 | imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
378 |
379 | // integrate rotation
380 | double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
381 | imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
382 | imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
383 | imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
384 | imuTime[imuPointerCur] = currentImuTime;
385 | ++imuPointerCur;
386 | }
387 |
388 | --imuPointerCur;
389 |
390 | if (imuPointerCur <= 0)
391 | return;
392 |
393 | cloudInfo.imuAvailable = true;
394 | }
395 |
396 | void odomDeskewInfo()
397 | {
398 | cloudInfo.odomAvailable = false;
399 |
400 | while (!odomQueue.empty())
401 | {
402 | if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
403 | odomQueue.pop_front();
404 | else
405 | break;
406 | }
407 |
408 | if (odomQueue.empty())
409 | return;
410 |
411 | if (odomQueue.front().header.stamp.toSec() > timeScanCur)
412 | return;
413 |
414 | // get start odometry at the beinning of the scan
415 | nav_msgs::Odometry startOdomMsg;
416 |
417 | for (int i = 0; i < (int)odomQueue.size(); ++i)
418 | {
419 | startOdomMsg = odomQueue[i];
420 |
421 | if (ROS_TIME(&startOdomMsg) < timeScanCur)
422 | continue;
423 | else
424 | break;
425 | }
426 |
427 | tf::Quaternion orientation;
428 | tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
429 |
430 | double roll, pitch, yaw;
431 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
432 |
433 | // Initial guess used in mapOptimization
434 | cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
435 | cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
436 | cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
437 | cloudInfo.initialGuessRoll = roll;
438 | cloudInfo.initialGuessPitch = pitch;
439 | cloudInfo.initialGuessYaw = yaw;
440 |
441 | cloudInfo.odomAvailable = true;
442 |
443 | // get end odometry at the end of the scan
444 | odomDeskewFlag = false;
445 |
446 | if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
447 | return;
448 |
449 | nav_msgs::Odometry endOdomMsg;
450 |
451 | for (int i = 0; i < (int)odomQueue.size(); ++i)
452 | {
453 | endOdomMsg = odomQueue[i];
454 |
455 | if (ROS_TIME(&endOdomMsg) < timeScanEnd)
456 | continue;
457 | else
458 | break;
459 | }
460 |
461 | if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
462 | return;
463 |
464 | Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
465 |
466 | tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
467 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
468 | Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
469 |
470 | Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
471 |
472 | float rollIncre, pitchIncre, yawIncre;
473 | pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
474 |
475 | odomDeskewFlag = true;
476 | }
477 |
478 | void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
479 | {
480 | *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
481 |
482 | int imuPointerFront = 0;
483 | while (imuPointerFront < imuPointerCur)
484 | {
485 | if (pointTime < imuTime[imuPointerFront])
486 | break;
487 | ++imuPointerFront;
488 | }
489 |
490 | if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
491 | {
492 | *rotXCur = imuRotX[imuPointerFront];
493 | *rotYCur = imuRotY[imuPointerFront];
494 | *rotZCur = imuRotZ[imuPointerFront];
495 | } else {
496 | int imuPointerBack = imuPointerFront - 1;
497 | double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
498 | double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
499 | *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
500 | *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
501 | *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
502 | }
503 | }
504 |
505 | void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
506 | {
507 | *posXCur = 0; *posYCur = 0; *posZCur = 0;
508 |
509 | // If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
510 |
511 | // if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
512 | // return;
513 |
514 | // float ratio = relTime / (timeScanEnd - timeScanCur);
515 |
516 | // *posXCur = ratio * odomIncreX;
517 | // *posYCur = ratio * odomIncreY;
518 | // *posZCur = ratio * odomIncreZ;
519 | }
520 |
521 | PointType deskewPoint(PointType *point, double relTime)
522 | {
523 | if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
524 | return *point;
525 |
526 | double pointTime = timeScanCur + relTime;
527 |
528 | float rotXCur, rotYCur, rotZCur;
529 | findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
530 |
531 | float posXCur, posYCur, posZCur;
532 | findPosition(relTime, &posXCur, &posYCur, &posZCur);
533 |
534 | if (firstPointFlag == true)
535 | {
536 | transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
537 | firstPointFlag = false;
538 | }
539 |
540 | // transform points to start
541 | Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
542 | Eigen::Affine3f transBt = transStartInverse * transFinal;
543 |
544 | PointType newPoint;
545 | newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
546 | newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
547 | newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
548 | newPoint.intensity = point->intensity;
549 |
550 | return newPoint;
551 | }
552 |
553 | void projectPointCloud()
554 | {
555 | int cloudSize = laserCloudIn->points.size();
556 | // range image projection
557 | for (int i = 0; i < cloudSize; ++i)
558 | {
559 | PointType thisPoint;
560 | thisPoint.x = laserCloudIn->points[i].x;
561 | thisPoint.y = laserCloudIn->points[i].y;
562 | thisPoint.z = laserCloudIn->points[i].z;
563 | thisPoint.intensity = laserCloudIn->points[i].intensity;
564 |
565 | float range = pointDistance(thisPoint);
566 | if (range < lidarMinRange || range > lidarMaxRange)
567 | continue;
568 |
569 | int rowIdn = laserCloudIn->points[i].ring;
570 | // int rowIdn = (i % 64) + 1 ; // for MulRan dataset, Ouster OS1-64 .bin file, giseop
571 |
572 | if (rowIdn < 0 || rowIdn >= N_SCAN)
573 | continue;
574 |
575 | if (rowIdn % downsampleRate != 0)
576 | continue;
577 |
578 | float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
579 |
580 | static float ang_res_x = 360.0/float(Horizon_SCAN);
581 | int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
582 | if (columnIdn >= Horizon_SCAN)
583 | columnIdn -= Horizon_SCAN;
584 |
585 | if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
586 | continue;
587 |
588 | if (rangeMat.at(rowIdn, columnIdn) != FLT_MAX)
589 | continue;
590 |
591 | thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
592 |
593 | rangeMat.at(rowIdn, columnIdn) = range;
594 |
595 | int index = columnIdn + rowIdn * Horizon_SCAN;
596 | fullCloud->points[index] = thisPoint;
597 | }
598 | }
599 |
600 | void cloudExtraction()
601 | {
602 | int count = 0;
603 | // extract segmented cloud for lidar odometry
604 | for (int i = 0; i < N_SCAN; ++i)
605 | {
606 | cloudInfo.startRingIndex[i] = count - 1 + 5;
607 |
608 | for (int j = 0; j < Horizon_SCAN; ++j)
609 | {
610 | if (rangeMat.at(i,j) != FLT_MAX)
611 | {
612 | // mark the points' column index for marking occlusion later
613 | cloudInfo.pointColInd[count] = j;
614 | // save range info
615 | cloudInfo.pointRange[count] = rangeMat.at(i,j);
616 | // save extracted cloud
617 | extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
618 | // size of extracted cloud
619 | ++count;
620 | }
621 | }
622 | cloudInfo.endRingIndex[i] = count -1 - 5;
623 | }
624 | }
625 |
626 | void publishClouds()
627 | {
628 | cloudInfo.header = cloudHeader;
629 | cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
630 | pubLaserCloudInfo.publish(cloudInfo);
631 | }
632 | };
633 |
634 | int main(int argc, char** argv)
635 | {
636 | ros::init(argc, argv, "lio_sam");
637 |
638 | ImageProjection IP;
639 |
640 | ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
641 |
642 | ros::MultiThreadedSpinner spinner(3);
643 | spinner.spin();
644 |
645 | return 0;
646 | }
647 |
--------------------------------------------------------------------------------
/SC-LIO-SAM/src/imuPreintegration.cpp:
--------------------------------------------------------------------------------
1 | #include "utility.h"
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 |
16 | #include
17 | #include
18 |
19 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
20 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot)
21 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
22 |
23 | class TransformFusion : public ParamServer
24 | {
25 | public:
26 | std::mutex mtx;
27 |
28 | ros::Subscriber subImuOdometry;
29 | ros::Subscriber subLaserOdometry;
30 |
31 | ros::Publisher pubImuOdometry;
32 | ros::Publisher pubImuPath;
33 |
34 | Eigen::Affine3f lidarOdomAffine;
35 | Eigen::Affine3f imuOdomAffineFront;
36 | Eigen::Affine3f imuOdomAffineBack;
37 |
38 | tf::TransformListener tfListener;
39 | tf::StampedTransform lidar2Baselink;
40 |
41 | double lidarOdomTime = -1;
42 | deque imuOdomQueue;
43 |
44 | TransformFusion()
45 | {
46 | if(lidarFrame != baselinkFrame)
47 | {
48 | try
49 | {
50 | tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
51 | tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
52 | }
53 | catch (tf::TransformException ex)
54 | {
55 | ROS_ERROR("%s",ex.what());
56 | }
57 | }
58 |
59 | subLaserOdometry = nh.subscribe("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
60 | subImuOdometry = nh.subscribe(odomTopic+"_incremental", 2000, &TransformFusion::imuOdometryHandler, this, ros::TransportHints().tcpNoDelay());
61 |
62 | pubImuOdometry = nh.advertise(odomTopic, 2000);
63 | pubImuPath = nh.advertise ("lio_sam/imu/path", 1);
64 | }
65 |
66 | Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
67 | {
68 | double x, y, z, roll, pitch, yaw;
69 | x = odom.pose.pose.position.x;
70 | y = odom.pose.pose.position.y;
71 | z = odom.pose.pose.position.z;
72 | tf::Quaternion orientation;
73 | tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
74 | tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
75 | return pcl::getTransformation(x, y, z, roll, pitch, yaw);
76 | }
77 |
78 | void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
79 | {
80 | std::lock_guard lock(mtx);
81 |
82 | lidarOdomAffine = odom2affine(*odomMsg);
83 |
84 | lidarOdomTime = odomMsg->header.stamp.toSec();
85 | }
86 |
87 | void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
88 | {
89 | // static tf
90 | static tf::TransformBroadcaster tfMap2Odom;
91 | static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
92 | tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));
93 |
94 | std::lock_guard lock(mtx);
95 |
96 | imuOdomQueue.push_back(*odomMsg);
97 |
98 | // get latest odometry (at current IMU stamp)
99 | if (lidarOdomTime == -1)
100 | return;
101 | while (!imuOdomQueue.empty())
102 | {
103 | if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
104 | imuOdomQueue.pop_front();
105 | else
106 | break;
107 | }
108 | Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
109 | Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
110 | Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
111 | Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
112 | float x, y, z, roll, pitch, yaw;
113 | pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
114 |
115 | // publish latest odometry
116 | nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
117 | laserOdometry.pose.pose.position.x = x;
118 | laserOdometry.pose.pose.position.y = y;
119 | laserOdometry.pose.pose.position.z = z;
120 | laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
121 | pubImuOdometry.publish(laserOdometry);
122 |
123 | // publish tf
124 | static tf::TransformBroadcaster tfOdom2BaseLink;
125 | tf::Transform tCur;
126 | tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
127 | if(lidarFrame != baselinkFrame)
128 | tCur = tCur * lidar2Baselink;
129 | tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
130 | tfOdom2BaseLink.sendTransform(odom_2_baselink);
131 |
132 | // publish IMU path
133 | static nav_msgs::Path imuPath;
134 | static double last_path_time = -1;
135 | double imuTime = imuOdomQueue.back().header.stamp.toSec();
136 | if (imuTime - last_path_time > 0.1)
137 | {
138 | last_path_time = imuTime;
139 | geometry_msgs::PoseStamped pose_stamped;
140 | pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
141 | pose_stamped.header.frame_id = odometryFrame;
142 | pose_stamped.pose = laserOdometry.pose.pose;
143 | imuPath.poses.push_back(pose_stamped);
144 | while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
145 | imuPath.poses.erase(imuPath.poses.begin());
146 | if (pubImuPath.getNumSubscribers() != 0)
147 | {
148 | imuPath.header.stamp = imuOdomQueue.back().header.stamp;
149 | imuPath.header.frame_id = odometryFrame;
150 | pubImuPath.publish(imuPath);
151 | }
152 | }
153 | }
154 | };
155 |
156 | class IMUPreintegration : public ParamServer
157 | {
158 | public:
159 |
160 | std::mutex mtx;
161 |
162 | ros::Subscriber subImu;
163 | ros::Subscriber subOdometry;
164 | ros::Publisher pubImuOdometry;
165 |
166 | bool systemInitialized = false;
167 |
168 | gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
169 | gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
170 | gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
171 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
172 | gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
173 | gtsam::Vector noiseModelBetweenBias;
174 |
175 |
176 | gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
177 | gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;
178 |
179 | std::deque imuQueOpt;
180 | std::deque imuQueImu;
181 |
182 | gtsam::Pose3 prevPose_;
183 | gtsam::Vector3 prevVel_;
184 | gtsam::NavState prevState_;
185 | gtsam::imuBias::ConstantBias prevBias_;
186 |
187 | gtsam::NavState prevStateOdom;
188 | gtsam::imuBias::ConstantBias prevBiasOdom;
189 |
190 | bool doneFirstOpt = false;
191 | double lastImuT_imu = -1;
192 | double lastImuT_opt = -1;
193 |
194 | gtsam::ISAM2 optimizer;
195 | gtsam::NonlinearFactorGraph graphFactors;
196 | gtsam::Values graphValues;
197 |
198 | const double delta_t = 0;
199 |
200 | int key = 1;
201 |
202 | gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
203 | gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
204 |
205 | IMUPreintegration()
206 | {
207 | subImu = nh.subscribe (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
208 | subOdometry = nh.subscribe("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
209 |
210 | pubImuOdometry = nh.advertise (odomTopic+"_incremental", 2000);
211 |
212 | boost::shared_ptr p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
213 | p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
214 | p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
215 | p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
216 | gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias
217 |
218 | priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
219 | priorVelNoise = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
220 | priorBiasNoise = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
221 | correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
222 | correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
223 | noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
224 |
225 | imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
226 | imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization
227 | }
228 |
229 | void resetOptimization()
230 | {
231 | gtsam::ISAM2Params optParameters;
232 | optParameters.relinearizeThreshold = 0.1;
233 | optParameters.relinearizeSkip = 1;
234 | optimizer = gtsam::ISAM2(optParameters);
235 |
236 | gtsam::NonlinearFactorGraph newGraphFactors;
237 | graphFactors = newGraphFactors;
238 |
239 | gtsam::Values NewGraphValues;
240 | graphValues = NewGraphValues;
241 | }
242 |
243 | void resetParams()
244 | {
245 | lastImuT_imu = -1;
246 | doneFirstOpt = false;
247 | systemInitialized = false;
248 | }
249 |
250 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
251 | {
252 | std::lock_guard lock(mtx);
253 |
254 | double currentCorrectionTime = ROS_TIME(odomMsg);
255 |
256 | // make sure we have imu data to integrate
257 | if (imuQueOpt.empty())
258 | return;
259 |
260 | float p_x = odomMsg->pose.pose.position.x;
261 | float p_y = odomMsg->pose.pose.position.y;
262 | float p_z = odomMsg->pose.pose.position.z;
263 | float r_x = odomMsg->pose.pose.orientation.x;
264 | float r_y = odomMsg->pose.pose.orientation.y;
265 | float r_z = odomMsg->pose.pose.orientation.z;
266 | float r_w = odomMsg->pose.pose.orientation.w;
267 | bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
268 | gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
269 |
270 |
271 | // 0. initialize system
272 | if (systemInitialized == false)
273 | {
274 | resetOptimization();
275 |
276 | // pop old IMU message
277 | while (!imuQueOpt.empty())
278 | {
279 | if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
280 | {
281 | lastImuT_opt = ROS_TIME(&imuQueOpt.front());
282 | imuQueOpt.pop_front();
283 | }
284 | else
285 | break;
286 | }
287 | // initial pose
288 | prevPose_ = lidarPose.compose(lidar2Imu);
289 | gtsam::PriorFactor priorPose(X(0), prevPose_, priorPoseNoise);
290 | graphFactors.add(priorPose);
291 | // initial velocity
292 | prevVel_ = gtsam::Vector3(0, 0, 0);
293 | gtsam::PriorFactor priorVel(V(0), prevVel_, priorVelNoise);
294 | graphFactors.add(priorVel);
295 | // initial bias
296 | prevBias_ = gtsam::imuBias::ConstantBias();
297 | gtsam::PriorFactor priorBias(B(0), prevBias_, priorBiasNoise);
298 | graphFactors.add(priorBias);
299 | // add values
300 | graphValues.insert(X(0), prevPose_);
301 | graphValues.insert(V(0), prevVel_);
302 | graphValues.insert(B(0), prevBias_);
303 | // optimize once
304 | optimizer.update(graphFactors, graphValues);
305 | graphFactors.resize(0);
306 | graphValues.clear();
307 |
308 | imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
309 | imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
310 |
311 | key = 1;
312 | systemInitialized = true;
313 | return;
314 | }
315 |
316 |
317 | // reset graph for speed
318 | if (key == 100)
319 | {
320 | // get updated noise before reset
321 | gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
322 | gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
323 | gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
324 | // reset graph
325 | resetOptimization();
326 | // add pose
327 | gtsam::PriorFactor priorPose(X(0), prevPose_, updatedPoseNoise);
328 | graphFactors.add(priorPose);
329 | // add velocity
330 | gtsam::PriorFactor 
