Added test_apriltags2.launch, updated rtabmap.launch with arguments tag_linear_variance (default 0.0001) and tag_angular_variance (default 9999, not used)

2026-10-03 16:27:46 +08:00 · 2019-01-06 17:08:12 -05:00
parent 5787d862f9
commit 02ec32b908
5 changed files with 112 additions and 4 deletions
@@ -101,6 +101,8 @@
  <arg name="gps_topic"                default="/gps/fix" />         <!-- gps async subscription -->
  
  <arg name="tag_topic"                default="/tag_detections" />  <!-- apriltags async subscription -->
+  <arg name="tag_linear_variance"      default="0.0001" />
+  <arg name="tag_angular_variance"     default="9999" />             <!-- >=9999 means ignore rotation in optimization, when rotation estimation of the tag is not reliable -->
  
  <!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
  <arg if="$(arg compressed)"     name="rgb_topic_relay"           default="$(arg rgb_topic)_relay"/>
@@ -257,7 +259,8 @@
      <param name="config_path"          type="string" value="$(arg cfg)"/>
      <param name="queue_size"           type="int" value="$(arg queue_size)"/>
      <param name="scan_normal_k"        type="int" value="$(arg scan_normal_k)"/>
-      
+      <param name="landmark_linear_variance"   type="double" value="$(arg tag_linear_variance)"/>
+      <param name="landmark_angular_variance"  type="double" value="$(arg tag_angular_variance)"/>      
      
      <remap from="rgb/image"       to="$(arg rgb_topic_relay)"/>
      <remap from="depth/image"     to="$(arg depth_topic_relay)"/>
@@ -280,9 +283,9 @@
      <remap from="odom"                   to="$(arg odom_topic)"/>
      
      <!-- localization mode -->
-	  <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
-	  <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
-	  <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
@@ -0,0 +1,14 @@
+# AprilTags 2 code parameters
+# Find descriptions in apriltags2/include/apriltag.h:struct apriltag_detector
+#                      apriltags2/include/apriltag.h:struct apriltag_family
+tag_family:        'tag36h11' # options: tag36h11, tag36h10, tag25h9, tag25h7, tag16h5
+tag_border:        1          # default: 1
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_refine_decode: 0          # default: 0
+tag_refine_pose:   0          # default: 0
+tag_debug:         0          # default: 0
+# Other parameters
+publish_tf:        true       # default: false
@@ -0,0 +1,65 @@
+# # Definitions of tags to detect
+#
+# ## General remarks
+#
+# - All length in meters
+# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
+#
+# ## Standalone tag definitions
+# ### Remarks
+#
+# - name is optional
+#
+# ### Syntax
+#
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+  ]
+# ## Tag bundle definitions
+# ### Remarks
+#
+# - name is optional
+# - x, y, z have default values of 0 thus they are optional
+# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
+#
+# ### Syntax
+#
+# tag_bundles:
+#   [
+#     {
+#       name: 'CUSTOM_BUNDLE_NAME',
+#       layout:
+#         [
+#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
+#           ...
+#         ]
+#     },
+#     ...
+#   ]
+tag_bundles:
+   [
+     {
+       name: 'tag_bundle1',
+       layout:
+         [
+           # pixel = 180mm/30 = 6mm
+           {id: 1,   size: 0.0480, x: 0.0000, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 2,   size: 0.0480, x: 0.0600, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 3,   size: 0.0480, x: 0.1200, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 25,  size: 0.0480, x: 0.0000, y: -0.0600, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 26,  size: 0.0480, x: 0.0600, y: -0.0600, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 27,  size: 0.0480, x: 0.1200, y: -0.0600, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 49,  size: 0.0480, x: 0.0000, y: -0.1200, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 50,  size: 0.0480, x: 0.0600, y: -0.1200, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 51,  size: 0.0480, x: 0.1200, y: -0.1200, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 73,  size: 0.0480, x: 0.0000, y: -0.1800, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 74,  size: 0.0480, x: 0.0600, y: -0.1800, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 75,  size: 0.0480, x: 0.1200, y: -0.1800, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+         ]
+     },
+   ]
@@ -0,0 +1,26 @@
+<launch>
+  <!-- Print on the file tag_bundle1.png (with GIMP: set width in Image Settings to 180.00mm) -->
+  <!-- Definition of the tag bundle is tags.yaml, make sure the camera is calibrated or adjust the size of the tag if needed -->
+  <!-- The TF published by apriltags_ros should match the point cloud created by the camera -->
+  <!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and tag_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
+
+  <!-- $ roslaunch realsense2_camera rs_camera.launch align_depth:=true -->
+  <!-- $ roslaunch rtabmap_ros test_apriltags2.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
+  <!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" tag_angular_variance:=9999 -->
+
+  <arg name="camera_frame_id"         default="camera_color_optical_frame"/>
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+
+  <!-- Set parameters -->
+  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltags2_ros_continuous_node" />
+  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltags2_ros_continuous_node" />
+  
+  <node pkg="apriltags2_ros" type="apriltags2_ros_continuous_node" name="apriltags2_ros_continuous_node" clear_params="true" output="screen">
+    <remap from="image_rect" to="$(arg rgb_topic)" />
+    <remap from="camera_info" to="$(arg camera_info_topic)" />
+
+    <param name="camera_frame" type="str" value="$(arg camera_frame_id)" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
+  </node>
+</launch>