/
multi_camera_mapping.launch
163 lines (143 loc) · 9.22 KB
/
multi_camera_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
<launch>
<!-- Multi-cameras demo with 3 Realsense Cameras -->
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=Frame-to-Map 1=Frame-to-Frame
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-"odom_info_data" : Fill odometry info messages with inliers/outliers data.
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="80" />
<arg name="inlier_distance" default="0.1" />
<arg name="local_map" default="3000" />
<arg name="odom_info_data" default="true" />
<arg name="wait_for_transform" default="true" />
<!-- publish statif tf from map to odom(for debugging only) -->
<!-- <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 1 map odom 50" /> -->
<!-- sync rgb/depth images per camera -->
<group ns="camera1">
<node pkg="nodelet" type="nodelet" name="nodelet_manager1" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync1" args="load rtabmap_ros/rgbd_sync nodelet_manager1">
<remap from="rgb/image" to="/realsense/color/image_raw"/>
<remap from="depth/image" to="/realsense/depth/image_rect_raw"/>
<remap from="rgb/camera_info" to="/realsense/color/camera_info"/>
<param name="approx" value="false"/>
</node>
</group>
<group ns="camera2">
<node pkg="nodelet" type="nodelet" name="nodelet_manager2" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager2">
<remap from="rgb/image" to="/realsense2/color/image_raw"/>
<remap from="depth/image" to="/realsense2/depth/image_rect_raw"/>
<remap from="rgb/camera_info" to="/realsense2/color/camera_info"/>
<param name="approx" value="false"/>
</node>
</group>
<group ns="camera3">
<node pkg="nodelet" type="nodelet" name="nodelet_manager3" args="manager"/>
<node pkg="nodelet" type="nodelet" name="rgbd_sync3" args="load rtabmap_ros/rgbd_sync nodelet_manager3">
<remap from="rgb/image" to="/realsense3/color/image_raw"/>
<remap from="depth/image" to="/realsense3/depth/image_rect_raw"/>
<remap from="rgb/camera_info" to="/realsense3/color/camera_info"/>
<param name="approx" value="false"/>
</node>
</group>
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry">
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<remap from="rgbd_image2" to="/camera3/rgbd_image"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="3"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="publish_tf" type="bool" value="false"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="OdomF2M/BundleAdjustment" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras-->
<param name="Vis/FeatureType" type="string" value="$(arg feature)"/>
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="Vis/CorNNType" type="string" value="$(arg nn)"/>
<param name="Vis/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomF2M/MaxSize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
<param name="odom_tf_linear_variance" value="0.0001"/>
<param name="odom_tf_angular_variance" value="0.0003"/>
<param name="Mem/DepthAsMask" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="50"/>
<param name="Kp/RoiRatios" type="string" value="0 0 0 0.25"/>
<param name="RGBD/OptimizeMaxError" type="string" value="2"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" for mapping, for localization don't put it. -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="3"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="map_negative_poses_ignored" type="bool" value="false"/> <!-- refresh grid map even if we are not moving-->
<param name="map_negative_scan_empty_ray_tracing" type="bool" value="false"/> <!-- don't fill empty space between the generated scans-->
<param name="queue_size" type="int" value="100"/>
<param name="database_path" type="string" value="~/.ros/rtabmap6.db"/>
<remap from="odom" to="/odometry/filtered"/>
<!-- <remap from="odom" to="/ground_truth/odom"/> -->
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<remap from="rgbd_image2" to="/camera3/rgbd_image"/>
<param name="Grid/FromDepth" type="string" value="true"/>
<param name="Vis/EstimationType" type="string" value="0"/> <!-- should be 0 for multi-cameras -->
<param name="Vis/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Vis/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="Vis/CorGuessWinSize" type="string" value="0"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/AngularUpdate" type="string" value="0.01"/>
<param name="RGBD/LinearUpdate" type="string" value="0.01"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
<param name="Reg/Force3DoF" type="string" value="true"/>
<param name="RGBD/LinearSpeedUpdate" type="string" value="0.03"/>
<param name="RGBD/AngularSpeedUpdate" type="string" value="0.03"/>
<param name="Optimizer/GravitySigma" type="string" value="0.2"/>
<param name="publish_tf" type="bool" value="true"/>
<param name="Kp/MaxFeatures" type="string" value="500"/>
<param name="tf_delay" type="double" value="0.02"/>
<!-- <param name="odom_frame_id" type="string" value="odom"/> -->
<param name="odom_tf_linear_variance" value="0.0025"/>
<param name="odom_tf_angular_variance" value="0.0003"/>
<param name="Mem/DepthAsMask" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="50"/>
<param name="Kp/RoiRatios" type="string" value="0 0 0 0.45"/>
<param name="RGBD/OptimizeMaxError" type="string" value="2"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="rgbd_cameras" type="int" value="3"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgbd_image0" to="/camera1/rgbd_image"/>
<remap from="rgbd_image1" to="/camera2/rgbd_image"/>
<remap from="rgbd_image2" to="/camera3/rgbd_image"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
</launch>