The column series is as follows:
Code framework
lio-sam:.
│ CMakeLists.txt #Project engineering configuration file, you can know which third-party libraries the author uses
│ LICENSE #Software copyright
│ package.xml #ROS package profile
│ README.md #Project description documents: document composition, dependence, operation, etc
│
├─config
│ │ params.yaml #Parameter file
│ │
│ └─doc #Storage renderings, flow charts, papers, etc
│ │
│ └─kitti2bag #Convert kitti dataset to bag format
│ kitti2bag.py
│ README.md
│
├─include
│ utility.h #Parameter server class, initialization parameters; Various common functions
│
├─launch
│ │ run.launch #Total run launch file
│ │
│ └─include #Sub module operation file
│ │ module_loam.launch
│ │ module_navsat.launch
│ │ module_robot_state_publisher.launch
│ │ module_rviz.launch
│ │
│ ├─config #Store rviz parameter files and robot coordinate system parameters
│ │ rviz.rviz
│ │ robot.urdf.xacro
│
├─msg
│ cloud_info.msg #Custom ROS data format
│
├─src #source file
│ featureExtraction.cpp #Extract radar line and surface features and publish radar point cloud
│ imageProjection.cpp #Subscribe to the extracted radar point cloud, IMU data and IMU odometer data, correct the distortion of the radar, and publish the position and attitude rough estimation of the odometer at the front end of the radar (at IMU frequency)
│ imuPreintegration.cpp #IMU pre integration, subscribe to radar odometer and IMU data, estimate IMU offset, optimize the graph of radar odometer and IMU pre integration factor, and output IMU odometer.
│ mapOptmization.cpp #Subscribe to radar front-end information and GPS information, carry out point cloud registration, and optimize the map of radar odometer, global GPS and loop detection factors.
│
└─srv
save_map.srvThe following is a schematic diagram of the code framework:
Here we mainly interpret the four source files in the src folder, as well as utility H and params Yaml file.
utility.h
It mainly includes ParamServer class and other functions.
Many public member variables are defined in the ParamServer class to store the parameters read by the parameter server, mainly including Topics, Frames, GPS Settings, Save pcd, Lidar Sensor Configuration, IMU, loop, voxel filter paprams, CPU Params, rounding map, Loop closure and global map visualization radius
Its constructor ParamServer() reads the parameters in the parameter server (params.yaml) and coexists them in the public member variable.
Classes in other source files inherit the ParamServer class. When instantiating an object, the constructor ParamServer of the parent class is called and the parameters are read.
The advantage of this is that the parameters that need to be changed frequently are written in the yaml file, and the program leaves an interface to read the parameters, which is convenient for debugging.
Other functions:
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
Rotate the IMU data to the front upper left coordinate system and judge whether the nine axis IMU is used because the attitude information of the magnetometer is used.
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
This is the publish point cloud function.
double ROS_TIME(T msg)
This is a function of printing ROS time.
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
Read acceleration data.
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
Turn the RPY angle of IMU to the RPY angle defined by ROS.
float pointDistance(PointType p)
Calculate the distance of the point.
float pointDistance(PointType p1, PointType p2)
Calculate the distance between two points.
params.yaml
The parameters of the parameter file are as follows:
- Topics class (the topic of each sensor is changed according to the sensor topic used)
pointCloudTopic: "points_raw" # Point cloud data imuTopic: "imu_correct" # IMU data odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file Frames Class (different coordinate systems) lidarFrame: "base_link" #Base coordinates baselinkFrame: "base_link" #Base coordinates odometryFrame: "odom" #Odometer coordinate system mapFrame: "map" #World coordinate system
- GPS class (useImuHeadingInitialization and useGpsElevation are true if GPS is used)
# GPS Settings useImuHeadingInitialization: true # if using GPS data, set to "true" useGpsElevation: false # if GPS elevation is bad, set to "false" gpsCovThreshold: 2.0 # m^2, threshold for using GPS data poseCovThreshold: 25.0 # m^2, threshold for using GPS data
- Save data class
# Export settings savePCD: false # Set to true when saving data savePCDDirectory: "/Downloads/LOAM/" # The save path must have / at the beginning and end, and it is under the root directory. Example: if a data file is created in the root directory and saved in the data folder, the parameter is / data/
- Radar parameter class (changed according to the model of radar used)
# Sensor Settings sensor: velodyne # lidar sensor type, either 'velodyne' or 'ouster' N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128) Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1 lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
- IMU Settings class (change the noise and offset of IMU and the value of gravity acceleration)
imuAccNoise: 3.9939570888238808e-03 imuGyrNoise: 1.5636343949698187e-03 imuAccBiasN: 6.4356659353532566e-05 imuGyrBiasN: 3.5640318696367613e-05 imuGravity: 9.80511 imuRPYWeight: 0.01 //Weight of magnetometer
- External parameter matrix (radar to inertial navigation, as explained in the previous article)
# Extrinsics (lidar -> IMU)
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [1, 0, 0,
0, 1, 0,
0, 0, 1]
extrinsicRPY: [1, 0, 0,
0, 1, 0,
0, 0, 1]- Loam parameter setting (the author uses the parameters of the radar odometer at the front end of loam, mainly the number and threshold of corner points and edge points)
# LOAM feature threshold edgeThreshold: 1.0 surfThreshold: 0.1 edgeFeatureMinValidNum: 10 surfFeatureMinValidNum: 100
- Voxel filter parameters (cube size)
# voxel filter paprams odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor mappingSurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
- Robot limit parameters (limit Z axis and rotation)
# robot motion constraint (in case you are using a 2D robot) z_tollerance: 1000 # meters rotation_tollerance: 1000 # radians
- CPU Params
numberOfCores: 4 # Number of cores for mapping optimization mappingProcessInterval: 0.15 # Seconds, regular mapping frequency
- Map parameters (keyframe threshold and point cloud density, keyframe sliding window distance)
# Surrounding map surroundingkeyframeAddingDistThreshold: 1.0 # meters, regulate keyframe adding threshold surroundingkeyframeAddingAngleThreshold: 0.2 # radians, regulate keyframe adding threshold surroundingKeyframeDensity: 2.0 # meters, downsample surrounding keyframe poses surroundingKeyframeSearchRadius: 50.0 # meters, within n meters scan-to-map optimization (when loop closure disabled)
- Loopback parameters (whether to enable loopback, loopback frequency, search range, key frame size, quantity, etc.)
# Loop closure loopClosureEnableFlag: true loopClosureFrequency: 1.0 # Hz, regulate loop closure constraint add frequency surroundingKeyframeSize: 50 # submap size (when loop closure enabled) historyKeyframeSearchRadius: 15.0 # meters, key frame that is within n meters from current pose will be considerd for loop closure historyKeyframeSearchTimeDiff: 30.0 # seconds, key frame that is n seconds older will be considered for loop closure historyKeyframeSearchNum: 25 # number of hostory key frames will be fused into a submap for loop closure historyKeyframeFitnessScore: 0.3 # icp threshold, the smaller the better alignment
- Visualization parameters (global map density, range)
# Visualization globalMapVisualizationSearchRadius: 1000.0 # meters, global map visualization radius globalMapVisualizationPoseDensity: 10.0 # meters, global map visualization keyframe density globalMapVisualizationLeafSize: 1.0 # meters, global map visualization cloud density
- GPS parameters (detailed parameters are not understood here)
Mainly navsat and ekf_gps series parameters
The next chapter writes the front end of LIO-SAM.