博客

apriltags2_ros应用实践——如何在realsense d435i上使用apriltags二维码实现定位

  • Post author:
  • Post category:其他




前言

最近课题中采用VINS-Mono来做视觉惯性融合定位（VIO），因为所要求的环境条件无法进行闭环，为消除累计误差，打算以每多少米放置一个二维码的方式来辅助SLAM定位。为此选择了apriltags。

至于为什么在那么多种二维码中选择apriltags（其他常用的二维码还有QR、ARtag等），apriltags是如何进行二维码识别与定位的，以及如何将apriltags得到的位姿融合到VIO中，待以后再述。这里主要想介绍的是以下内容：

1、如何成功跑通apriltags2_ros，并发布一个可在RVIZ可视化的odometry topic。

2、如何将apriltags得到的定位位姿转换到VINS坐标系上，使二者的结果对齐。

可能大家并不了解VINS-Mono，或者VIO甚至SLAM，所幸本文内容何其关系也不大，而坐标系转换（TF）对于其他应用也具有借鉴意义。

关于我博客对apriltags2_ros源码与配置文件的一些修改，已上传到我的github上，供各位参考。




https://github.com/ManiiXu/Apriltags2_VO



安装环境

系统：ubuntu 16.04 内核 4.15.0-43-generic

ROS： kinetic

相机：intel realsense d435i

已安装相机驱动：librealsense 2.17.1 以及相机ROS Wrapper，相机能够正常工作

已编译VINS，并使用数据集成功运行。



apriltags2_ros的编译

我这里直接用的是apriltags2，源码

https://github.com/AprilRobotics/apriltag_ros

wiki提供了非常详细的教程：

http://wiki.ros.org/apriltags2_ros/Tutorials/Detection in a video stream

安装过程非常简单：

将源码放到你的工作空间，如catkin_ws/src中，然后编译即可使用 

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

运行的时候，在打开摄像头之后，输入： 

roslaunch apriltags2_ros continuous_detection.launch



apriltags2_ros的配置

当然你在跑之前还得修改launch和yaml文件。先简单解释以下launch文件： 

<!-- Set parameters -->
  <rosparam command="load" file="$(find apriltags2_ros)/config/settings.yaml" ns="$(arg node_namespace)" />
  <rosparam command="load" file="$(find apriltags2_ros)/config/tags.yaml" ns="$(arg node_namespace)" />

这里是描述程序中读取相关配置文件地址的地方，不需要修改，可以看到会有两个yaml文件：settings.yaml与tags.yaml，具体内容下面说。 

<node pkg="apriltags2_ros" type="apriltags2_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="camera_frame" type="str" value="$(arg camera_frame)" />
    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
  </node>

这里是ROS标准的启动节点（node）写法，不过不同的是他做了一个重映射（remap）。这里主要需要修改的订阅的topic。程序中需要订阅相机图像数据与相机信息，比如对于我的realsense d435i，在开启相机时会发布两个相关的topic：/camera/image_raw与/camera/camera_info，因此只需将以下内容进行如下修改即可： 

<arg name="camera_name" default="/camera" />
<arg name="camera_frame" default="image_raw" />

如果你用的是usb相机，并且还未对相机进行内参标定，那么需要先使用camera_calibration包完成标定，然后把标定得到的yaml文件放到摄像头驱动读取的目录下。usb_cam会读取标定文件，并通过camera_info的消息发布出去。你也可以在源程序上进行修改，直接将相机内参写到程序中来代替通过topic订阅的方式读取。

然后是settings.yaml文件，该文件主要是对程序一些基础参数进行配置，包括采用何种apriltags二维码（tag_family），是否需要发布tf（publish_tf）等。具体每一条的解释可以直接看上面的wiki，因为一般不需要进行修改我就不详细介绍了。

最后是tags.yaml文件，该文件主要是定义你需要在图像上识别的tag ID，没有在该文件中定义的tag ID在运行时会被忽略。需要注意的还有：

1、同一ID的tag不能该配置文件中以不同的大小出现两次，也不能出现在一张图片的两个地方。显然，这些都将在检测中产生歧义。

2、只要具有相同的大小，在standalone_tags和tag_bundles中列出具有相同ID的标记就可以了。

3、确保打印的tag至少包含1位宽的白色边框，AprilTag2算法对周围的白色边框进行采样。

因为目的是通过apriltags定位，只需要修改tag_bundles这块，定义tag ID的格式如下：（这里位置和四元数这样写只是作为一个例子，后面我会介绍这种情况的物理意义） 

tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.143, x: 0.0000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707},
          {id: 1, size: 0.143, x: 1.2000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707},
          {id: 2, size: 0.143, x: 2.4000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707},
          {id: 3, size: 0.143, x: 3.6000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707},
          {id: 4, size: 0.143, x: 4.8000, y: 0.0000, z: 0.0, qw: 0.707, qx: 0.0, qy: 0.0, qz: -0.707}
        ]
    }
  ]



apriltags2_ros的运行

至此配置文件已经搞定，打开apriltags2_ros。 

roslaunch apriltags2_ros continuous_detection.launch

使用rostopic list可以看到程序发布了/tag_detections相关的topic：（没有/tag_Odometry）

在这里插入图片描述

其中/tag_detections就是apriltags2_ros最后输出的定位结果， 

rostopic echo /tag_detections

可以看到如果没有检测到apriltags2二维码的时候只有header信息而没有detections：

在这里插入图片描述

而在检测到apriltags2的情况下，/tag_detections会输出位姿，而/tag_detections_image会在输入的图像上标记出apriltags2的ID：

在这里插入图片描述



发布odometry

/tag_detections作为一个由apriltags2_ros自己定义的消息类型，无法使用RVIZ进行可视化看到相机的位姿。因此在apriltags2_ros源码中，我又创建了一个topic：”/tag_Odometry”。它会发布类型为nav_msgs::Odometry的消息，其内容包括xyz的三维坐标和姿态的四元数，主要对源码进行如下修改： 

odomtry_publisher_ = nh.advertise<nav_msgs::Odometry>("tag_Odometry", 100);

// Publish detected tags in the image by AprilTags 2
  AprilTagDetectionArray tag_detection_array(tag_detector_->detectTags(cv_image_,camera_info));
  tag_detections_publisher_.publish(tag_detection_array);

  if (true) {
    for (unsigned int i = 0; i < tag_detection_array.detections.size(); i++) {
      nav_msgs::Odometry odometry;
      odometry.header = tag_detection_array.detections[i].pose.header;
      //odometry.header.frame_id = "my_bundle";
      odometry.pose.pose = tag_detection_array.detections[i].pose.pose.pose;
      odomtry_publisher_.publish(odometry);
    }
  }

重新编译后，打开RVIZ可得到相机姿态的可视化（箭头或者坐标轴）

在这里插入图片描述



apriltags2_ros的输出

需要注意的是，这里apriltags2_ros最后给出的结果，其实是从二维码所在坐标系到相机坐标系的变换矩阵的



R

t

a

g

c

a

m

,

t

t

a

g

c

a

m

R^{cam}_{tag},t^{cam}_{tag}







R











t


a


g










c


a


m






​












,





t











t


a


g










c


a


m






​















，要将其用于作为相机的位姿估计



R

,

t

R,t






R


,




t





，还需要进行转换：





R

=

(

R

t

a

g

c

a

m

)

T

,

t

=

−

(

R

t

a

g

c

a

m

)

T

t

t

a

g

c

a

m

R =(R^{cam}_{tag})^T,t=-(R^{cam}_{tag})^Tt^{cam}_{tag}






R




=








(



R











t


a


g










c


a


m






​













)










T









,




t




=








−


(



R











t


a


g










c


a


m






​













)










T










t











t


a


g










c


a


m






​

















具体的坐标变换问题（即如何与VINS的位姿对齐）将留到下一个博客照着代码解释。


版权声明：本文为qq_41839222原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。