/
icp-slam-live_demo_RPLIDAR_gridmap.ini
126 lines (101 loc) · 5.09 KB
/
icp-slam-live_demo_RPLIDAR_gridmap.ini
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#------------------------------------------------------------
# Config file for the "icp-slam-live" application
# See: https://www.mrpt.org/list-of-mrpt-apps/application-icp-slam-live/
#------------------------------------------------------------
# =======================================================================
# LIDAR SENSOR: This section must be named "LIDAR_SECTION" for
# the app icp-slam-live. See docs and examples of how to set up
# other LIDAR sensors in [MRPT]/share/mrpt/config_files/rawlog-grabber/
# =======================================================================
[LIDAR_SENSOR]
driver = CRoboPeakLidar
process_rate = 60 // (Hz) Set to some rate above the max sensor rate
COM_port_WIN = COM10
COM_port_LIN = /dev/ttyUSB0
sensorLabel = LASER1
pose_x = 0 // Laser range scaner 3D position in the robot (meters)
pose_y = 0
pose_z = 0
pose_yaw = 0 // Angles in degrees
pose_pitch = 0
pose_roll = 0
#preview = true // Enable GUI visualization of captured data
# Optional: Exclusion zones to avoid the robot seeing itself:
#exclusionZone1_x = 0.20 0.30 0.30 0.20
#exclusionZone1_y = 0.20 0.30 0.30 0.20
# Optional: Exclusion zones to avoid the robot seeing itself:
#exclusionAngles1_ini = 20 // Deg
#exclusionAngles1_end = 25 // Deg
#=======================================================
# Section: [ICP]
# Parameters of ICP inside the ICP-based SLAM class
#=======================================================
[ICP]
maxIterations = 80 // The maximum number of iterations to execute if convergence is not achieved before
minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated:
minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated:
thresholdDist = 0.10 // Initial maximum distance for matching a pair of points
thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points.
ALFA = 0.5 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching)
smallestThresholdDist=0.04 // This is the smallest the distance threshold can become after stopping ICP and accepting the result.
onlyClosestCorrespondences=true // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower)
onlyUniqueRobust = false // Force unique correspondences in both directions when pairing two point clouds
# 0: icpClassic
# 1: icpLevenbergMarquardt
ICP_algorithm = icpClassic
# decimation to apply to the point cloud being registered against the map
# Reduce to "1" to obtain the best accuracy
corresponding_points_decimation = 2
#=======================================================
# Section: [MappingApplication]
# Use: Here comes global parameters for the app.
#=======================================================
[MappingApplication]
# The directory where the log files will be saved (left in blank if no log is required)
logOutput_dir=LOG_ICP-SLAM
LOG_FREQUENCY=500 // The frequency of log files generation:
SAVE_3D_SCENE=1
SAVE_POSE_LOG=0
CAMERA_3DSCENE_FOLLOWS_ROBOT=1
SHOW_PROGRESS_3D_REAL_TIME=1
SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS=5
SHOW_LASER_SCANS_3D = true
# Save raw sensor data for optional post-processing offline?
SAVE_RAWLOG = true
localizationLinDistance = 0.2 // The distance threshold for correcting odometry with ICP (meters)
localizationAngDistance = 5 // The distance threshold for correcting odometry with ICP (degrees)
insertionLinDistance = 1.2 // The distance threshold for inserting observations in the map (meters)
insertionAngDistance = 45.0 // The distance threshold for inserting observations in the map (degrees)
minICPgoodnessToAccept = 0.40 // Minimum ICP quality to accept correction [0,1].
# Neeeded for LM method, which only supports point-map to point-map matching.
matchAgainstTheGrid = 1
# ========================================================
# MULTIMETRIC MAP CONFIGURATION
# See docs for (Google for) mrpt::maps::CMultiMetricMap
# ========================================================
# Creation of maps:
occupancyGrid_count=1
pointsMap_count=0
# ====================================================
# MULTIMETRIC MAP: PointsMap #00
# ====================================================
# Creation Options for PointsMap 00:
[MappingApplication_pointsMap_00_insertOpts]
minDistBetweenLaserPoints = 0.01
fuseWithExisting = false
isPlanarMap = 1
# ====================================================
# MULTIMETRIC MAP: OccGrid #00
# ====================================================
# Creation Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_creationOpts]
resolution=0.04
disableSaveAs3DObject=0
# Insertion Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_insertOpts]
mapAltitude=0
useMapAltitude=false
maxDistanceInsertion=30
maxOccupancyUpdateCertainty=0.54
considerInvalidRangesAsFreeSpace=1
minLaserScanNoiseStd=0.001