使用ar_track_alvar持续跟踪AR tag

安装

  • ROS: ar_track_alvar 安装 git clone https://github.com/machinekoder/ar_track_alvar.git -b noetic-develgit clone https://github.com/ros-perception/ar_track_alvar.git -b noetic-develcatkin build

  • 生成二维码

    • rosrun ar_track_alvar createMarker 90

准备工作(获得相机参数)

通过launch文件启动节点

  • 使用USB摄像头进行测试 注意:cam_image_topiccam_info_topic应改成对应参数 output_frame应为TF中存在的坐标系(如map/world/camera_link),这样才能将二维码位置注册到该坐标系下

    <launch>
     
        <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
            <param name="video_device" value="/dev/video0" />
            <param name="image_width" value="1280" />
            <param name="image_height" value="720" />
            <param name="pixel_format" value="yuyv" />
            <param name="framerate" value="30" />
            <!--<param name="autofocus" value="false" />-->
            <!--<param name="focus" value="0" />-->
            <!--<param name="contrast" value="32" />-->
            <!--<param name="brightness" value="32" />-->
            <!--<param name="saturation" value="32" />-->
            <!--<param name="sharpness" value="22" />-->
            <!--<param name="focus" value="0" />-->
            <!--<param name="camera_frame_id" value="usb_cam" />-->
            <param name="io_method" value="mmap"/>
        </node>
     
        <arg name="frame_prefix"         default="ar_marker_" />
        <arg name="marker_size"          default="5.0" />
        <arg name="max_new_marker_error" default="0.2" />
        <arg name="max_track_error"      default="0.2" />
     
        <arg name="cam_image_topic"      default="/usb_cam/image_raw" />
        <arg name="cam_info_topic"       default="/usb_cam/camera_info" />
        <arg name="output_frame"         default="/map" />
     
        <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
            <param name="marker_size"           type="double" value="$(arg marker_size)" />
            <param name="max_new_marker_error"  type="double" value="$(arg max_new_marker_error)" />
            <param name="max_track_error"       type="double" value="$(arg max_track_error)" />
            <param name="output_frame"          type="string" value="$(arg output_frame)" />
            <param name="frame_prefix"          type="string" value="$(arg frame_prefix)" />
     
            <remap from="camera_image"  to="$(arg cam_image_topic)" />
            <remap from="camera_info"   to="$(arg cam_info_topic)" />
        </node>
     
        <node pkg="tf" type="static_transform_publisher" name="camera_fix" args="0 0 0 0 0 0 1 map head_camera 50"/>
     
        <node pkg="rviz" type="rviz" name="ar_test_rviz"/>
    </launch>
  • 在运行launch文件之后,打开Rviz,添加TF,同时重新选择Global Options中的Fixed Frame

  • 使用Realsense时启动文件的参考 output_frame(类型为TF)、camera_image_topic(类型为sensor_msgs/Image)、camera_info_topic(类型为sensor_msgs/CameraInfo)需要改成与相机的topic/tf对应的参数

<arg name="marker_size" default="10" />
<arg name="max_new_marker_error" default="0.08" />
<arg name="max_track_error" default="0.2" />
<arg name="output_frame" default="/camera_color_optical_frame" />
<arg name="camera_image_topic" default="/camera/color/image_raw" />
<arg name="camera_info_topic" default="/camera/color/camera_info" />
<arg name="frame_prefix" default="ar_marker_" />
 
<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen">
    <param name="marker_size" type="double" value="$(arg marker_size)" />
    <param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
    <param name="max_track_error" type="double" value="$(arg max_track_error)" />
    <param name="output_frame" type="string" value="$(arg output_frame)" />
    <param name="frame_prefix" type="string" value="$(arg frame_prefix)" />
    <remap from="camera_image" to="$(arg camera_image_topic)" />
    <remap from="camera_info" to="$(arg camera_info_topic)" />
</node>

坐标系

  • 二维码坐标系原点为其中心,z轴从纸面向外。注意相机会有多个坐标系,camera_link和camera_depth_optical_frame、camera_depth_frame、camera_color_optical_frame、camera_color_frame根据相机不同会有所区别

其它注意事项:

  • bar code的检测需要黑色正方形及足够宽的白色边框,如果将其嵌入其它颜色的边框中会出现识别问题