ros下实现usb相机功能，并基于apriltag_ros实现定位

1.安装usb_cam软件包

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

安装完成后可以启动相机

roslaunch usb_cam usb_cam-test.launch

launch文件在 /opt/ros/kinetic/share/usb_cam/launch 目录下，launch文件如下：

可通过 ls /dev/video* 命令查看当前的相机设备，相应的更改launch文件中的配置/dev/video1，对应设备号，启动自己的相机设备。

2.使用camera_calibration软件包进行相机内参标定

• 安装相机标定软件包

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

• 棋盘格下载
  相机的标定是以一个由黑白方块组成的棋盘为基准进行的，如下图所示。可以从本文的附件中下载8×6国际象棋棋盘，并打印出来后将其贴到一个平坦的纸板或者墙面上。作为参考，8×6棋盘横向有9个方块，所以有8个交叉点，而竖向有7个方块，有6个交叉点，所以它被称为8×6棋盘。
  
• 启动usb相机

roslaunch usb_cam usb_cam-test.launch

• 启动标定节点

rosrun camera_calibration cameracalibrator.py  --size 8x6 --square 0.029 image:=/usb_cam/image_raw camera:=/usb_cam

size :标定棋盘个的内部角点个数，这里实用的棋盘一共有6行，每行有8个内部角点（不包括最外围的一圈棋盘格）。
squre：这个参数对应每一个棋盘格的边长，单位是米，棋盘格打印出来以后要自己再测量一下打印的是或否准确然后自己更改，我打印的棋盘方格大小是29mm的，故squre为0.029。
image和camera：设置摄像头发布的图像话题。

• 标定节点运行后会运行GUI，此时如果用相机对准棋盘，校准将立即开始。在GUI屏幕的右侧，可以看到一个标有X、Y、Size和Skew的条形控件。这是校准的进展状态，都以绿色填满意味着校准完成。在校准过程中需要将棋盘对着相机朝着左/右/上/下/前/后移动，还需要倾斜棋盘，这是个比较无聊的过程，我们需要拿着校准棋盘不断的移动，晃荡个大概5分钟左右就差不多了，看到基本上绿色已经填满了即可。校准所需的所有图像都记录下来之后，CALIBRATE按钮会被激活。点击这个按钮后会进行实际的校准计算，这需要大约3到5分钟。此时就可以将校准板放下了，耐心等待计算完成后，点击SAVE按钮保存校准信息，存储的地址显示在执行校准的终端窗口中，是存储在某一个/tmp目录（如“/tmp/calibrationdata.tar.gz”）中。
• 大佬的这篇博客讲的很清楚，可直接参考此链接，我就不再赘述了。

3.apriltag安装

• 下载源码，官方链接（https://github.com/AprilRobotics/apriltag）

git clone https://github.com/AprilRobotics/apriltag

• 编译安装

cd apriltag
cmake .
sudo make install

官方链接里面有不同的安装方式，我只是选择了其中的一种安装方式。

4.apriltags2_ros的编译

apriltag_ros源码：源码链接
详细的wiki教程：wiki教程
安装过程：

mkdir -p apriltag_ws/src
git clone https://github.com/AprilRobotics/apriltag_ros.git
cd ..
catkin_make

编译完成之后，确保usb相机标定完成并且启动之后（roslaunch usb_cam usb_cam-test.launch），运行continuous_detection.launch文件。

roslaunch apriltags2_ros continuous_detection.launch

5.apriltags2_ros的配置

• 参数说明
  （1）tag_family (string, 默认值: tag36h11)
  可用于检测的apriltag系列。一次只能检测一种标签，不支持多种标签的同时检测。支持: tag36h11, tag36h10, tag25h9, tag25h7 and tag16h5.
  （2）tag_border (int, 默认值: 1)
  标签外部黑色边框的宽度，标签中每个小正方形的边长为1位。
  （3）tag_threads (int, 默认值: 4)
  AprilTag 2核心算法应使用多少个线程？
  （4）tag_decimate (double, 默认值: 1.0)
  可以在较低分辨率的图像上进行四边形的检测，从而以提高姿势精度和稍微降低检测率的速度提高速度。仍以全分辨率对二进制有效负载进行解码和姿态估计。
  （5）tag_blur (double, 默认值: 0.0)
  应该对分割后的图像应用哪种高斯模糊。参数是像素的标准偏差。噪点较多的图片用非零值比较合适（例如0.8），大于0是模糊图像，小于0是锐化图像。
  （6）tag_refine_edges (int, 默认值: 1)
  如果不为零，则将每个四边形的边缘调整为“捕捉”到附近的强渐变。当采用抽取时，这很有用，因为它可以大大提高初始四边形估计的质量。提高了角点检测的精度，从而提高了姿态估计的精度，通常建议开启。如果tag_decimate == 1.0，则忽略该选项。
  （7）tag_refine_decode (int, 默认值: 0)
  当非零时，增加检测到的标签数量，增加假阴性检测率。对于分辨率分辨率阈值附近的很小的标签特别有效。
  （8）tag_refine_pose (int, 默认值: 0)
  当非零时，提高提取位姿的精度，但消耗计算资源。这是通过最大化标签黑白边框周围的对比度来完成的。尽管不如tag_refine_decode那样有效（或快速），但这通常会增加成功检测到的标签的数量。
  （9）publish_tf (bool, 默认值: false)
  启用在/t话题上发布标签相机相对位姿，在rviz中实现可视化。
  （10）camera_frame (string, 默认值: camera)
  （11）publish_tag_detections_image (bool, 默认值: false)
  启用发布/tag_detections_image话题。
• launch文件的配置
  launch文件如下：

读取两个yaml文件的地方不需要修改：setting.yaml与tags.yaml
ROS标准的启动节点（node）写法，不过不同的是这里做了一个重映射（remap）。我们主要需要修改的订阅的topic。程序中需要订阅相机图像数据与相机信息。外接usb相机启动后会发布两个相关的topic：/usb_cam/camera_info和/usb_cam/image_raw
在此之前我们已经对usb_camera进行了相机标定，得到了camra_info的信息。
camra_info话题的理解可以看我这篇文章

• settings.yaml文件的配置（配置apriltag算法）
  该文件主要是对程序一些基础参数进行配置，包括采用何种apriltags二维码（tag_family），是否需要发布tf（publish_tf）等。保持默认，一般不需要修改。

• tags.yaml文件的配置（定义要查找的标签和标签簇）
  （1）同一ID的tag不能该配置文件中以不同的大小出现两次，也不能出现在一张图片的两个地方。显然，这些都将在检测中产生歧义，确保打印的标签至少被1位宽的白色边框包围。
  （2）只要具有相同的大小，在standalone_tags（单标签）和tag_bundles（标签簇）中列出具有相同ID的标记就可以了，标签簇的检测比单个标签的检测更加准确。
  （3）确保打印的tag至少包含1位宽的白色边框，AprilTag2算法对周围的白色边框进行采样。
  配置示例：

standalone_tags:
  [
    {id: 0, size: 0.05},
    {id: 1, size: 0.05},
    {id: 2, size: 0.05},
    {id: 3, size: 0.05},
    {id: 4, size: 0.05}
  ]

tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.05, x: 0.0000, y: 0.0000, z: 0.0000, qw: 1.0000, qx: 0.0000, qy: 0.0000, qz: 0.0000},
          {id: 4, size: 0.05, x: 0.0548, y: -0.0522, z: -0.0057, qw: 1.0000, qx: 0.0063, qy: -0.0016, qz: 0.0010},
          {id: 3, size: 0.05, x: -0.0580, y: -0.0553, z: 0.0015, qw: 1.0000, qx: -0.0039, qy: -0.0006, qz: 0.0016},
          {id: 2, size: 0.05, x: 0.0543, y: 0.0603, z: -0.0043, qw: 1.0000, qx: -0.0016, qy: 0.0082, qz: 0.0018},
          {id: 1, size: 0.05, x: -0.0582, y: 0.0573, z: 0.0012, qw: 1.0000, qx: -0.0057, qy: -0.0028, qz: 0.0006}
        ]
    }
  ]

6.apriltags2_ros的运行

• 整体工作方式如下图所示：
  
  订阅的topic：
  （1）/camera/image_rect：包含图像的sensor_msgs/Image话题（例如，来自摄像机的视频流的帧）。 假定图像是无畸变的，即由针孔照相机产生。 笔记本电脑网络摄像头通常已经提供了这种图像，而其他相机可能需要中间节点（例如image_proc）进行处理，我购买的是无畸变的外接usb摄像头。
  （2）/camera/camera_info: sensor_msgs/CameraInfo话题，其中包含/camera/camera_info/K中的相机内参矩阵。可以通过使用例如 camera_calibration 工具包进行相机标定。
  发布的topic：
  （1）/tf：摄像机坐标与每个检测到的标签或标签簇的坐标之间的相对位姿关系。仅在publish_tf：config / settings.yaml中为true时才发布/tf。
  （2）/tag_detections：与/tf话题提供的信息相同，但作为自定义消息，其中包含标签ID，大小和geometry_msgs/PoseWithCovarianceStamped姿势信息。
  （3）/tag_detections_image：与/camera/image_rect输入的图像相同，但突出显示检测到的标签。仅在launch/continuous_detection.launch中的publish_tag_detections_image==true时才发布。

• 现在运行连续标签检测器：

roslaunch apriltags2_ros continuous_detection.launch

使用rostopic list可以看到程序发布了/tag_detections相关的topic
**该算法输出的信息是相机在二维码坐标系下的精确位姿与方向。**包括平移矢量position（也就是位置坐标）与旋转四元数orientation（也就是朝向）。我最终希望得到二维码相对于我们的移动机器人的位姿，因此需要进行坐标变换。
其中/tag_detections就是apriltags2_ros最后输出的定位结果，截取其中一帧的结果：

header: 
  seq: 10416
  stamp: 
    secs: 1610698673
    nsecs: 558949073
  frame_id: "usb_cam"
detections: 
  - 
    id: [10]
    size: [0.15]
    pose: 
      header: 
        seq: 10652
        stamp: 
          secs: 1610698673
          nsecs: 558949073
        frame_id: "usb_cam"
      pose: 
        pose: 
          position: 
            x: 0.0408862981401
            y: -0.265477154272
            z: 0.73617112622
          orientation: 
            x: -0.191289591513
            y: 0.827204092907
            z: 0.512393188318
            w: 0.128821199427
        covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

还可以在riviz可视化界面上显示检测结果，如下图所示：

参考链接
生成apriltag标签