-
Notifications
You must be signed in to change notification settings - Fork 10
/
mapping.launch
265 lines (214 loc) · 15.2 KB
/
mapping.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<rosparam file="$(find robotino_simulations)/config/camera.yaml" command="load"/>
<!-- Arguments from main.launch -->
<arg name="database_path" default="rtabmap.db"/>
<arg name="sim" default="true"/>
<arg name="delete" default=""/>
<arg name="cuda" default="true"/>
<arg name="compressed" default="false"/>
<arg name="image_transport" default="compressed"/>
<arg name="robust" default="false"/>
<arg name="gt" default=""/>
<!-- Arguments for rtabmap and other nodes -->
<arg name="camera_info_topic" default="/camera2/color/camera_info"/> <!-- /camera_info -->
<arg name="rgb_topic" default="/camera2/color/image_raw"/> <!-- todo try compressed -->
<arg name="depth_topic" default="/camera/depth/image_rect_raw" unless="$(arg sim)"/>
<arg name="depth_topic" default="/camera2/depth/image_raw" if="$(arg sim)"/>
<arg name="scanner" default="/robot/laser/scan" if="$(arg sim)"/>
<arg name="scanner" default="/scan" unless="$(arg sim)"/>
<arg name="visual_odometry" default="false"/>
<arg name="max_depth" default="4"/> <!-- max features depth-->
<arg name="min_depth" default="0.4"/> <!-- min features depth-->
<arg name="detector" default="2"/>
<group ns="rtabmap">
<arg name="use_sim_time" default="true"/>
<param name="/use_sim_time" value="$(arg use_sim_time)"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport"
args="$(arg image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)_relay"/>
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport"
args="$(arg image_transport)Depth in:=$(arg depth_topic) raw out:=$(arg depth_topic)_relay"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync" output="screen">
<remap from="rgb/image" to="$(arg rgb_topic)_relay" if="$(arg compressed)"/>
<remap from="depth/image" to="$(arg depth_topic)_relay" if="$(arg compressed)"/>
<remap from="rgb/image" to="$(arg rgb_topic)" unless="$(arg compressed)"/>
<remap from="depth/image" to="$(arg depth_topic)" unless="$(arg compressed)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="rgbd_image"/> <!-- output -->
<param name="approx_sync" value="true" unless="$(arg sim)"/>
<param name="approx_sync" value="false" if="$(arg sim)"/>
</node>
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg delete)">
<param name="Reg/Strategy" type="int"
value="2"/> <!-- 0=Visual, 1=ICP (1 requires scan)--> <!-- todo try 2, 0 ,1 -->
<param if="$(arg gt)" name="ground_truth_frame_id" value="world"/>
<param unless="$(arg gt)" name="ground_truth_frame_id" value=""/>
<param if="$(arg gt)" name="ground_truth_base_frame_id" value="base_footprint_gt"/>
<param unless="$(arg gt)" name="ground_truth_base_frame_id" value=""/>
<param name="queue_size" value="30"/>
<!-- Basic RTAB-Map Parameters -->
<param name="publish_tf" value="true"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="frame_id" type="string" value="base_link"/>
<!-- Basic RTAB-Map Parameters-->
<param name="odom_sensor_sync" value="true"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_rgb" type="bool" value="false"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_scan" type="bool" value="true"/>
<param name="approx_sync" value="true"/>
<param name="wait_for_transform_duration" type="double" value="0.1"/> <!-- todo refine -->
<!-- RTAB-Map Inputs -->
<remap from="scan" to="$(arg scanner)"/>
<remap from="rgbd_image" to="rgbd_image"/>
<remap from="odom" to="/odometry/filtered"/>
<!-- RTAB-Map Output -->
<remap from="grid_map" to="/map"/>
<!-- Rate (Hz) at which new nodes are added to map -->
<param name="Rtabmap/DetectionRate" type="string" value="2"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="bool" value="false"/>
<param name="Rtabmap/MemoryThr" type="string" value="0"/>
<param name="map_filter_radius" type="double" value="0.3"/> <!--only one node in the radius up to filter angle -->
<param name="map_filter_angle" type="double" value="20"/>
<param name="map_always_update" type="bool" value="false"/>
<param name="Rtabmap/TimeThr" type="string" value="500"/>
<!-- <param name="scan_range_max" type="double" value="4"/>-->
<param name="scan_range_min" type="double" value="0.3"/>
<param name="Rtabmap/scan_range_min" type="double" value="0.3"/>
<param name="Rtabmap/MaxRetrieved" type="string" value="20"/>
<param name="rtabmap/scan_range_min" type="double" value="0.3"/>
<param name="rtabmap_ros/scan_range_min" type="double" value="0.3"/>
<!-- <param name="scan_angle_increment" type="double" value="1"/>-->
<param name="Rtabmap/ImageBufferSize" type="double" value="2"/>
<param name="GFTT/BlockSize" type="string" value="4"/>
<param name="GFTT/K" type="string" value="0.04"/>
<param name="GFTT/MinDistance" type="string" value="5"/>
<param name="GFTT/QualityLevel" type="string" value="0.005"/>
<param name="GFTT/UseHarrisDetector" type="string" value="false"/>
<param name="Grid/RangeMax" type="double" value="$(arg max_depth)"/>
<param name="Grid/RangeMin" type="double" value="0.4"/>
<param name="Grid/FootprintHeight" type="double" value="0.8"/>
<param name="Grid/FootprintLength" type="double" value="0.4"/>
<param name="Grid/FootprintWidth" type="double" value="0.4"/>
<param name="Grid/FromDepth" type="string" value="true"/>
<param name="Grid/3D" type="string" value="true"/>
<param name="Grid/RayTracing" type="string" value="true"/>
<param name="Grid/MaxGroundHeight" type="double" value="0.1"/>
<param name="Grid/MinGroundHeight" type="double" value="-0.05"/>
<param name="Grid/NormalsSegmentation" type="string" value="false"/>
<param name="Grid/MaxGroundAngle" type="double" value="45"/>
<param name="Grid/NormalK" type="double" value="15"/>
<param name="Grid/ClusterRadius" type="double" value="0.05"/> <!-- 0.05-->
<param name="Grid/MinClusterSize" type="double" value="10"/> <!-- 5-->
<param name="Grid/ScanDecimation" type="string" value="0"/>
<param name="Grid/NoiseFilteringRadius" type="double" value="0.1"/>
<param name="Grid/NoiseFilteringMinNeighbors" type="double" value="5"/>
<param name="Grid/MaxObstacleHeight" type="double" value="1.5"/>
<param name="Grid/DepthDecimation" type="double" value="1"/>
<param name="Mem/ImagePreDecimation" type="double" value="2"/>
<param name="Mem/ImagePostDecimation" type="double" value="2"/>
<param name="Grid/CellSize" type="double" value="0.05"/>
<param name="GridGlobal/MinSize" type="string" value="30"/>
<param name="GridGlobal/MaxNodes" type="string" value="0"/>
<param name="GridGlobal/FootprintRadius" type="double" value="0"/> <!--fixme 0.4 not working D: D: -->
<param name="GridGlobal/UpdateError" type="double" value="0.05"/> <!-- 0.01 -->
<param name="GridGlobal/ProbClampingMin" type="double" value="0.001"/> <!-- 0.01 -->
<param name="GridGlobal/ProbClampingMax" type="double" value="0.999"/> <!-- 0.01 -->
<param name="GridGlobal/OccupancyThr" type="double" value="0.55"/> <!-- 0.01 -->
<param name="GridGlobal/ProbHit" type="double" value="0.8"/> <!-- 0.01 -->
<param name="GridGlobal/ProbMiss" type="double" value="0.4"/> <!-- 0.01 -->
<param name="Kp/MinDepth" type="double" value="$(arg min_depth)"/>
<param name="Kp/MaxDepth" type="double" value="0"/>
<param name="Kp/MaxFeatures" type="string" value="500"/> <!-- todo try to increase -->
<param name="Vis/MaxFeatures" type="string" value="500"/> <!-- todo try to increase -->
<param name="Kp/SubPixIterations" type="string" value="2"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="Kp/BadSignRatio" type="string" value="0.4"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/BadignaturesIgnored" type="string" value="false"/>
<param name="Mem/RecentWmRatio" type="double" value="0.2"/>
<param name="Mem/ReduceGraph" type="bool" value="true"/>
<param name="Mem/RawDescriptorsKept" type="bool" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<param name="Mem/DepthAsMask" type="string" value="true"/>
<param name="Optimizer/Strategy" type="string" value="2"/> <!-- todo g2o=1, GTSAM=2 -->
<param name="g2o/Optimizer" type="string" value="0"/>
<param name="Optimizer/LadmarksIgnored" type="string" value="true"/>
<param name="Optimizer/Robust" type="string"
value="$(arg robust)"/> <!--Optimizer/Robust=false with RGBD/OptimizeMaxError=3 -->
<param name="RGBD/SavedLocalizationIgnored" type="bool" value="true"/>
<param name="RGBD/LocalBundleOnLoopClosure" type="bool" value="false"/> <!-- false -->
<param name="RGBD/LocalRadius" type="string" value="5"/>
<param name="RGBD/LoopClosureReextractFeatures" type="bool" value="false"/> <!--todo false maybe try -->
<param name="RGBD/MaxLocalRetrieved" type="string" value="10"/>
<param name="RGBD/OptimizeMaxError" type="string" value="0"
if="$(arg robust)"/> <!-- should be 0 if RGBD/OptimizeRobust is true -->
<param name="RGBD/OptimizeMaxError" type="double" value="3" unless="$(arg robust)"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="50"/> <!-- maybe increase -->
<param name="RGBD/ProximityMaxPaths" type="string" value="4"/>
<param name="RGBD/ProximityPathFilteringRadius" type="string" value="1"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="50"/>
<param name="RGBD/ProximityAngle" type="string" value="359"/>
<param name="RGBD/ProximityOdomGuess" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="bool" value="true"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<param name="RGBD/MaxLoopClosureDistance" type="string" value="2"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="RGBD/NewMapOdomChangeDistance" type="string" value="2"/>
<param name="Reg/Force3DoF" type="string" value="true"/> <!-- 2D SLAM todo check -->
<param name="Reg/RepeatOnce" type="string" value="false"/> <!-- 2D SLAM todo check -->
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/LocalImmunizationRatio" type="string" value="0.4"/>
<param name="Vis/SubPixIterations" type="string" value="0"/>
<param name="Vis/SubPixWinSize" type="string" value="3"/>
<param name="Vis/MaxDepth" type="string" value="0"/>
<param name="Vis/MinDepth" type="double" value="$(arg min_depth)"/>
<param name="Vis/MinInliers" type="string" value="15"/>
<param name="Vis/InlierDistance" type="string" value="0.5"/>
<param name="Vis/PnPReprojError" type="string" value="10"/>
<param name="Vis/PnPRefineIterations" type="string" value="5"/>
<param name="Vis/FeatureType" type="string" value="6"/> <!-- 8 -->
<param name="Vis/EpipolarGeometryVar" type="string" value="0.5"/> <!-- 8 -->
<param name="Vis/DepthAsMask" type="string" value="true"/>
<param name="Icp/RangeMin" type="double" value="0.4"/>
<param name="Icp/MaxTranslation" type="double" value="0.05"/>
<param name="Icp/MaxRotation" type="double" value="0.3"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/VoxelSize" type="string" value="0.0"/>
<param name="Icp/Epsilon" type="string" value="0.00001"/>
<param name="Icp/PointToPlaneK" type="string" value="5"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0.5"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.05"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/> <!-- use libpointmatcher to handle PointToPlane with 2d scans-->
<param name="Icp/PMOutlierRatio" type="string" value="0.9"/>
<param name="ORB/Gpu" type="bool" value="true"/>
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="log">
<param name="frame_id" type="string" value="base_link"/>
<param name="queue_size" value="1"/>
<param name="publish_tf" value="false"/>
<remap from="scan" to="$(arg scanner)"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="Odom/GuessMotion" type="string" value="true"/>
<param name="Odom/ResetCountdown" type="string" value="10"/>
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
</node>
<!-- visualization with rtabmapviz-->
<!-- <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz"-->
<!-- args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">-->
<!-- <remap from="odom" to="/odometry/filtered"/>-->
<!-- <param name="subscribe_depth" type="bool" value="false"/>-->
<!-- <param name="subscribe_rgb" type="bool" value="false"/>-->
<!-- <param name="subscribe_rgbd" type="bool" value="true"/>-->
<!-- <param name="subscribe_scan" type="bool" value="true"/>-->
<!-- <param name="frame_id" type="string" value="base_link"/>-->
<!-- <remap from="scan" to="$(arg scanner)"/>-->
<!-- </node>-->
</group>
<!-- Mapping Node -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
</launch>