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”)就是二维码类型,直接替换即可。其他参数使用默认值即可。

  1. 配置 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="" /> <!-- set to value="gdbserver localhost:10000" for remote debugging -->
  <arg name="node_namespace" default="apriltag_ros_continuous_node" />

  <!-- <arg name="camera_name" default="/camera_rect" /> -->
  <!-- <arg name="image_topic" default="image_rect" /> -->
  <arg name="camera_name" default="/camera/color" />
  <arg name="image_topic" default="image_raw" />

  <!-- Set parameters -->
  <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 topics from those used in code to those on the ROS network -->
    <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" />      <!-- default: false -->
  </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。有点类似内接圆和外切圆的关系。

评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值