Intel Realsense D435i + Apriltag_ros 实现对相机姿态的估计

这里使用 Apriltag_ros 功能包可以通过单张rgb图像来估计 相机在二维码坐标系上的 姿态。

软硬件：

• Intel Realsense D435i
• Ubuntu 18.04 Desktop

---

Step 1： 下载并编译依赖库 AprilTag 3

在这一步的时候如果直接git容易出现 Error: RPL 报错，比较稳妥的方式是下载他的 zip 包然后解压。

https://github.com/AprilRobotics/apriltag

$ cd apriltag
$ mkdir build
$ cd build
$ cmake ..
$ make -j4
$ make install

---

Step 2： 下载 apriltag_ros 包到工作空间下

apriltag_ros 这个包git是可行的，不会出现报错的情况，如果出现了还是直接下载zip就行 这里假定工作空间名字叫 temp

$ cd temp/src
$ git clone https://github.com/AprilRobotics/apriltag_ros
$ cd ..
$ catkin_make

---

Step 3：下载realsense的ROS包并编译

如果设备不是realsense的话也没关系，只要想办法让采集到的图像发布到ros的话题上就行。如果之前没装过realsense的话可能需要安装SDK，按照CSDN上的流程安装即可。

$ cd temp/src
$ git clone https://github.com/IntelRealSense/realsense-ros
$ cd ..
$ catkin_make

---

Step 4：启动realsense来查看有哪些话题。

将 realsense d435i 连接上电脑，一定要用 usb 3.0 的线，否则无法启动。

$ source devel/setup.bash
$ roslaunch realsense2_camera rs_camera.launch 

另开一个窗口查看现在激活的话题。

$ rostopic list

如果你的realsense正确打开了的话会出现一系列的内容，这里截取了其中一小部分：

$ 
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions

其中 /camera/color/camera_info 和 /camera/color/image_raw 是 apriltag_ros 所需要的，前者是相机信息，后者是realsense采集到的彩色三通道图片。

注意：apriltag_ros 只能使用三通到图片，有深度的图片是无法使用的。

---

Step 5：配置 apriltag_ros 的配置文件

1. 配置aprilatg_ros/config/setting.yaml。

这里我把注释删了，其他和原始文件相同。

tag_family:        'tag36h11' 
tag_threads:       2          
tag_decimate:      1.0       
tag_blur:          0.0        
tag_refine_edges:  1         
tag_debug:         0 
max_hamming_dist:  2 

publish_tf:        true
transport_hint:    "raw"  

tag_family:

使用的是什么种类的二维码，这个参数一定要正确，否则是无法检测出图像的。如何判断自己二维码是什么类型，在他的链接中 https://github.com/AprilRobotics/apriltag_ros Tag Size Definition 部分可以看到并排的6个二维码，下面的字符串（如：“Tag36h11” 和 “TagCircle21h7”）就是二维码类型，直接替换即可。其他参数使用默认值即可。

2. 配置 tags.yaml

这个文件只有两个标签可以编写。

standalone_tags:
  [
    {id: 001, size: 0.75, name: tag_1}
  ]

tag_bundles:
  [
    {
      name: 'tag_1',
      layout:
        [
          {id: 001, size: 0.75, x: 0, y: 0, z: 0, qw: 1, qx: 0, qy: 0, qz: 0}
        ]
    }
  ]

standalone_tags

这里面写要使用多少个二维码，apriltag_ros 允许一张图片中出现多个二维码，但一定要明确每个二维码类型，如果想要添加的话这样写就可以，但记得两个标签都要添加:

standalone_tags:
  [
    {id: 001, size: 0.75, name: tag_1},
    {id: 002, size: 0.20, name: tag_2},
    ...
  ]

tag_bundles:
  [
    {
        name: 'tag_1',
        layout:
        [
            {id: 001, size: 0.75, x: 0, y: 0, z: 0, qw: 1, qx: 0, qy: 0, qz: 0}
        ]
    },
    {
        name: 'tag_2',
        layout:
        [
            {id: 002, size: 0.20, x: 0, y: 0, z: 0, qw: 1, qx: 0, qy: 0, qz: 0}
        ]
    }
  ]

• id：你给每个二维码的编号，可以从任意数字开始，只要你自己能区分哪个号是哪个二维码就好；
• size：二维码的长度。这个值是需要手动测量出来的，不同类型的二维码测量方式不同，具体可以看他的链接 https://github.com/AprilRobotics/apriltag_ros Tag Size Definition 部分，红色箭头就是你需要手动侧脸的二维码长度，单位是米，然后填写到这里；
• name：和id一样，这是为了更好地区分可以任起；

tag_bundles

• name：和每个tag保持一致；
• id：和每个tag保持一致；
• size：和每个tag保持一致；
• x,y,z：这个指的是该二维码中心点坐标，在推算相机姿态的时候就是以这个数为坐标原点进行计算的。假设相机垂直于这个中心点1m，那么最后返回的pose中x，y，z=[0,0,1]；
• qw,qx,qy,qz：四元数，和上面的x,y,z效果差不多，如果二维码贴在了一个斜面上，那么这里一定要修改，否则计算出来的pose就会差出很多。

---

Step 6：修改 continuous_detection.launch 文件

打开 apriltag_ros/launch/continuous_detection.launch 文件。

需要修改的主要有两个标签：

• arg name=“camera_name”：相机话题的前缀，如 /camera/color
• arg name=“image_topic”：相机发布的图像话题，如 image_raw

这两个值是在 Step 4 中查看相机发布的话题中看见，如果你用的不是realsense，那么需要按照相机包发布出来的话题名修改，这里用的是默认设置的 realsense 相机话题。

注意：“camera_name” 一定只能用前缀，如果多加了 “/” 会导致算法订阅到的话题变成了 “/camera/color//image_raw” 这样是不会出数据的，因为后面其实是做了一个字符串拼接：

<remap from="image_rect" to="$(arg camera_name)/$(arg image_topic)" />
<remap from="camera_info" to="$(arg camera_name)/camera_info" />

最终修改如下：

<launch>
  <arg name="launch_prefix" default="" /> 
  <arg name="node_namespace" default="apriltag_ros_continuous_node" />

  
  
  <arg name="camera_name" default="/camera/color" />
  <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>

---

Step 7：启动 apriltag_ros 算法

新开一个窗口。

$ source devel/setup.bash
$ roslaunch apriltag_ros continuous_detection.launch

如果没有出现红色的报错说明启动成功了，如果有则检查 yaml 文件有没有出现多一个逗号或者省略号之类的。

然后再开一个窗口就可以订阅推算出来的话题：

$ rostopic echo /tag_detections

如果只出现这个说明二维码没有在相机视野内，或者前面二维码类型配置错误。

---
header: 
  seq: 1739
  stamp: 
    secs: 1664460149
    nsecs: 591933966
  frame_id: "camera_color_optical_frame"
detections: []
---

正确检测到的应该会有下面的信息：

---
header: 
  seq: 1612
  stamp: 
    secs: 1664460138
    nsecs: 717808008
  frame_id: "camera_color_optical_frame"
detections: 
  - 
    id: [0]
    size: [0.075]
    pose: 
      header: 
        seq: 3482
        stamp: 
          secs: 1664460138
          nsecs: 717808008
        frame_id: "camera_color_optical_frame"
      pose: 
        pose: 
          position: 
            x: 0.0454747857095
            y: -0.107662586406
            z: 0.540045888173
          orientation: 
            x: 0.999320052973
            y: 0.0235171876449
            z: -0.0148883373629
            w: -0.0241807986292
        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]
  - 
    id: [0]
    size: [0.078]
    pose: 
      header: 
        seq: 3482
        stamp: 
          secs: 1664460138
          nsecs: 717808008
        frame_id: "camera_color_optical_frame"
      pose: 
        pose: 
          position: 
            x: 0.0472937771379
            y: -0.111969089862
            z: 0.5616477237
          orientation: 
            x: 0.999320052973
            y: 0.0235171876449
            z: -0.0148883373629
            w: -0.0241807986292
        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]
---

注意：这里其实是有两个pose，仔细看两个pose标签中的 size 是不一样的，这个其实就是二维码边框一个像素的大小，如果你需要的是只算黑色部分就选小的pose，如果你需要的是紧挨着黑色部分的白色像素，那就选大的pose。有点类似内接圆和外切圆的关系。