ROS 摄像头校准与Apriltag标签使用

ROS 摄像头校准与Apriltag标签使用

最近在研究EAI的移动机器人，发现它里边是采用了Apriltag来识别物体位姿然后用dobot机械臂来进行抓取的，因此单独研究了下apriltag标签在ROS中的使用
遇到的坑就是不知道为啥ROS的摄像头校准界面没有校准的进度条出现，刚开始用的是/cv_camera（https://link.zhihu.com/?target=https%3A//wiki.ros.org/cv_camera）这个包来驱动摄像头，后来换成了usb_cam重新操作一遍才出现了校准界面。

ROS校准

camera_calibration包专门用于校准摄像头，校准操作得到的是包含畸变参数矩阵等的一个yaml配置文件。

安装摄像头驱动usb_cam

usb_cam是针对USB摄像头的ROS驱动包，刚开始用了另一个CV的驱动包，其实都类似的作用，就是把摄像头的图像等信息通过ROS话题的方式发布出来。
安装这个包：

sudo apt-get install ros-kinetic-usb-cam

我用的是kinetic版本的ros
他自带一个launch启动文件：

roslaunch usb_cam usb_cam-test.launch

<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="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="mjpeg" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  node>
  <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  node>
launch>

该launch文件可以启动usb_cam_node节点，发布摄像头话题，并且打开image-view工具节点，用于显示所发布的摄像头ROS话题，因此打开后应该会显示摄像头拍摄内容的界面。
需要注意的是有的摄像头是mjpeg格式，有的是yuy2格式（默认）不对的话就不显示画面，需要在launch文件的参数配置中修改。
param name=“video_device” value="/dev/video0" 参数用于指定发布的摄像头设备，可以用ls指令查看摄像头的设备名称，这里是video0

该launch文件启动的第一个节点就可以发布出所需的摄像头ROS话题信息。第二个节点启动可视化界面ui。因此在我们后续使用apriltag_ros不需要打开第二个可视化节点，只需要第一个节点发布的ROS摄像头话题即可。我在该目录下编写了一个新的launch文件（usb_cam-test2.launch）：

<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="640" />
    <param name="image_height" value="480" />
    <param name="pixel_format" value="mjpeg" />
    <param name="camera_frame_id" value="usb_cam" />
    <param name="io_method" value="mmap"/>
  node>
launch>

至此，启动新编写的launch文件：

roslaunch usb_cam usb_cam-test2.launch

当然也可以手动rosrun运行usb_cam_node节点，但是需要逐个输入众多的参数。
此时用rostopic list可以看到所发布的摄像头信息话题，如果打开rviz，增加image，选择准确的摄像头话题：/usb_cam/image_raw就可以看到摄像头拍摄的原始图像。

校准操作

能够正确发布摄像头信息后，就可以进行校准操作了。
安装校准包：

rosdep install camera_calibration

或者：

sudo apt-get install ros-kinetic-camera-calibration

运行校准包节点：

rosrun camera_calibration cameracalibrator.py ——size 9x6 ——square 0.023 image:=/usb_cam/image_raw camera:=/usb_cam

——size 10x7 表示所用棋盘格的格数，长x宽（不是方块个数，而是角点个数！），中间的是x不是星号
——square 0.023 表示每一个格的实际边长，单位是m，需要准确测量下
image:=/usb_cam/image_raw和camera:=/usb_cam 参考所发布的摄像头原始图像话题名称填写
在这里我遇到的问题就是显示出来的标定界面没有标定进度，可以看到图像，但是无法完成标定，后来试了第二次才可以，有大神知道原因可以告诉我。

在标定界面上下，左右，前后，旋转移动标定板，直到calibration按钮可以被按下，然后按下calibration按钮，耐心等待，这里会卡死一段时间，不要操作或者退出，然后在点击save，最后点击commit
刚开始用一个标定板，很卡，不能连续检测到角点，后来换了一个大的更清楚的，不卡了并且角点能够连续检测到！
commit按下后程序会自动将生成的yaml校准配置文件放入到usb_cam包启动时加载的file:///home/zdx/.ros/camera_info/head_camera.yaml目录下，以head_camera.yaml命名，那么下次在启动usb_cam节点时就会自动加载它了。

image_width: 640
image_height: 480
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [401.562265, 0.000000, 325.645184, 0.000000, 536.650162, 217.818521, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.424688, 0.120445, 0.005439, -0.006276, 0.000000]
rectification_matrix:
  rows: 3
  cols: 3
  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
projection_matrix:
  rows: 3
  cols: 4
  data: [259.826508, 0.000000, 315.350456, 0.000000, 0.000000, 484.122009, 215.044332, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

image_width、image_height代表图片的长宽
camera_name为摄像头名
camera_matrix规定了摄像头的内部参数矩阵
distortion_model指定了畸变模型
distortion_coefficients指定畸变模型的系数
rectification_matrix为矫正矩阵，一般为单位阵
projection_matrix为外部世界坐标到像平面的投影矩阵

apriltag_ros

首先安装apriltag_ros包，参考官方教程：
http://wiki.ros.org/apriltag_ros

apriltag_ros包安装

直接按照官方github下的教程建议即可，但是要安装apriltag_ros包必须首先下载另一个包：apriltag包git clone https://github.com/AprilRobotics/apriltag.git

mkdir -p ~/catkin_ws/src                # Make a new workspace 
cd ~/catkin_ws/src                      # Navigate to the source space
git clone https://github.com/AprilRobotics/apriltag.git      # Clone Apriltag library
git clone https://github.com/AprilRobotics/apriltag_ros.git  # Clone Apriltag ROS wrapper
cd ~/catkin_ws                          # Navigate to the workspace
rosdep install --from-paths src --ignore-src -r -y  # Install any missing packages
catkin build    # Build all packages in the workspace (catkin_make_isolated will work also)

最后一步我用的是catkin_make_isolated，试了catkin_make不行，不明白这两个的区别，有大神可以给解释下哈。

apriltag_ros 工作原理

该图片取自官方教程，描述了这个包的工作原理，即输入两个话题给这个包（1、原始图像话题。2、包含校准矩阵信息的摄像头信息话题），输出3个处理后的话题（1、包含tf信息的标准tf话题。2、包含位置与四元数姿态信息的话题。3、包含检测后高亮图片的图像话题。1和3话题可以在rviz中可视化显示）
apriltag_ros 包里的config配置文件夹下，有两个主要的yaml配置文件：

其中settings.yaml文件用于配置功能包的一些设置：

# AprilTag 3 code parameters
# Find descriptions in apriltag/include/apriltag.h:struct apriltag_detector
#                      apriltag/include/apriltag.h:struct apriltag_family
tag_family:        'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
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_debug:         0          # default: 0
max_hamming_dist:  2          # default: 2 (Tunable parameter with 2 being a good choice - values >=3 consume large amounts of memory. Choose the largest value possible.)
# Other parameters
publish_tf:        true       # default: false
transport_hint:    "raw"      # default: raw, see http://wiki.ros.org/image_transport#Known_Transport_Packages for options

tag_family表示二维码的种类，默认为tag36h11
publish_tf表示是否发布tf信息，默认为true
其他配置标签信息可以参考官方教程中：

Parameters
tag_family (string, default: tag36h11)
AprilTag family to use for detection. Supported: tag36h11, tag36h10, tag25h9, tag25h7 and tag16h5.
tag_border (int, default: 1)
Width of the tag outer (circumference) black border
tag_threads (int, default: 4)
How many threads should the AprilTag 2 core algorithm use?
tag_decimate (double, default: 1.0)
Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution. .
tag_blur (double, default: 0.0)
What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values (e.g. 0.8).
tag_refine_edges (int, default: 1)
When non-zero, the edges of the each quad are adjusted to “snap to” strong gradients nearby. This is useful when decimation is employed, as it can increase the quality of the initial quad estimate substantially. Generally recommended to be on (1). Very computationally inexpensive. Option is ignored if tag_decimate==1.0.
tag_refine_decode (int, default: 0)
When non-zero, detections are refined in a way intended to increase the number of detected tags. Especially effective for very small tags near the resolution threshold (e.g. 10px on a side).
tag_refine_pose (int, default: 0)
When non-zero, detections are refined in a way intended to increase the accuracy of the extracted pose. This is done by maximizing the contrast around the black and white border of the tag. This generally increases the number of successfully detected tags, though not as effectively (or quickly) as tag_refine_decode.
publish_tf (bool, default: false)
Enable publishing the tag-camera relative poses on the /tf topic
camera_frame (string, default: camera)
Camera frame name
publish_tag_detections_image (bool, default: false)
Enable publishing on the /tag_detections_image topic

tags.yaml文件则是配置具体的二维码参数，包括ID号，尺寸等信息，这里又分为两种类型：

# # 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:
  [
    {id: 0, size: 0.013, name: TEST_TAG_1},
    {id: 2, size: 0.013, name: TEST_TAG_2}
  ]
# ## 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:
  [
  ]

standalone_tags下边设置的是单一的标签，也就是每个标签都是独立的一个，可以设置多个。ID为标签的ID号，尺寸为标签的边长单位为米，名称为该标签所对应的frame名字，会显示在发布的标签tf frame中。
tag_bundles下边设置的是位姿相对固定的一组标签，除了ID号尺寸外还可以设置标签之间的相对位姿（这个我没有测试，只测试了单一标签）下图取自官方教程

官方例子里边的配置：

standalone_tags:
  [
    {id: 10, size: 0.15},
    {id: 20, size: 0.1},
    {id: 30, size: 0.07}
  ]
tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.05, x: 0.0000, y: 0.0000, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 4, size: 0.05, x: 0.0548, y: -0.0522, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 3, size: 0.05, x: -0.0580, y: -0.0553, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 2, size: 0.05, x: 0.0543, y: 0.0603, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 1, size: 0.05, x: -0.0582, y: 0.0573, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0}
        ]
    }
  ]

标签生成

接住了openMV的IDE生成标签：
https://link.zhihu.com/?target=https%3A//openmv.io/pages/download

这里可以选择之前配置的类型：TAG36H11

生成的标签：

使用apriltag识别功能

1、打开摄像头usb_cam节点：

roslaunch usb_cam usb_cam-test2.launch

2、启动apriltag

roslaunch apriltag_ros continuous_detection.launch

<launch>
  <arg name="launch_prefix" default="" /> 
  <arg name="node_namespace" default="apriltag_ros_continuous_node" />
  <arg name="camera_name" default="/usb_cam" />
  <arg name="image_topic" default="image_raw" />

  
  <rosparam command="load" file="$(find apriltag_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
  <rosparam command="load" file="$(find apriltag_ros)/config/tags.yaml" ns="$(arg node_namespace)" />
  
  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="$(arg node_namespace)" clear_params="true" output="screen" launch-prefix="$(arg launch_prefix)" >
    
    <remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
    <remap from="camera_info" to="$(arg camera_name)/camera_info" />

    <param name="publish_tag_detections_image" type="bool" value="true" />      
  node>
launch>

这个launch文件需要指定arg name=“camera_name” default="/usb_cam" 也就是摄像头话题的名称，以及arg name=“image_topic” default=“image_raw”
启动节点后，就会发现多了标签处理的输出话题：

使用rostopic echo /tag_detections,可以查看所发布的位置与姿态话题信息，这里需要注意，需要source这个包下的setup.bash文件，否则会报话题类型不存在的错误。

启动rviz：
add下添加image和tf并指定fixed frame为usb_cam（当然也可以为二维码对应的名称），就可以看到tf的坐标系相对位姿关系：

两个标签：

主要参考资料就是官方教程和几个国内的博客：
http://wiki.ros.org/apriltag_ros
https://zhuanlan.zhihu.com/p/289582482
https://www.guyuehome.com/22899