使用Apriltag2-ROS对相机进行定位

本文是对Manii博客，使用USB相机对其代码的复现

前言

使用Apriltag2 ROS版本对相机进行定位，主要思想是根据二维码，利用PNP的方法实现相机的定位。

代码主要实现如下功能：

1.根据二维码标签的位姿信息，实时定位相机的位姿信息(两者的位姿信息均为世界坐标系)

2.相机的坐标精度在1厘米以内

3.通过ROS自带的工具包Rviz实现相机位姿及路径的可视化

准备工作

1.系统：ubuntu 16.04

2.ROS：kinetic(ROS的安装教程查看ROS-kinetic)

3.USB摄像机驱动以及标定(若无请参考单目摄像头标定)

相机标定好的yaml文件一般会在/home/lab1712/.ros/camera_info路径下(可用Ctrl+H显示隐藏文件.ros)

4.下载Manii的源码

代码编译

1.将代码放入工作空间中，如catkin_ws1/src

2.由于Manii使用的intel realsense d435i相机，本人使用的USB相机，故而需修改源代码在/catkin_ws1/src/Apriltags2_VO/apriltags2_ros/launch文件夹下的continuous_detection.launch文件

修改为以下内容，目的是为了保证程序能订阅到相机发布的topic

3.对源码进行编译即可使用

cd catkin_ws1
catkin_make

代码运行

1.代码运行之前需要修改二维码标签在世界坐标系下的位姿，文件为catkin_ws1/src/Apriltags2_VO/apriltags2_ros/config路径下的tags.yaml文件

需要测量二维码的边长，A4纸打印出来测量的为0.139米，二维码的在世界坐标系下的位姿需根据自己的实际情况进行设定。我这里只修改了id_1的二维码进行测试

接下来按步骤启动程序就好

2.在Terminal中运行以下命令：

roscore

3.新开一个Terminal，打开摄像头

roslaunch usb_cam usb_cam-test.launch

4.新开一个Terminal，运行apriltags2_ros

cd catkin_ws1
source devel/setup.bash
roslaunch apriltags2_ros continuous_detection.launch

5.新开一个Terminal，运行标签检测节点

cd catkin_ws1
source devel/setup.bash
rostopic echo /tag_detections

此时如果USB相机捕捉到二维码时，已在命令窗口中实时显示相机的位姿信息(若未捕捉到二维码，不显示信息，如左图；若捕捉到二维码，实时显示信息如右图)

如果想可是话，还要运行如下命令

+1.新开一个Terminal，运行标签检测节点

cd catkin_ws1
source devel/setup.bash
rostopic echo /tag_detections_image

+2.新开一个Terminal，打开Ros的可视化工具包Rviz

rosrun rviz rviz

下面是Rviz的初始界面，需要注意两个圈红圈的地方

将红圈的map改为usb_cam，点击add按钮添加话题，首先把二维码检测的话题添加进去

二维码检测的话题添加进去后在左下角就可以看到被标记出来的二维码标签，再将路径和姿态两个话题添加进去

这事主图出现的是一个箭头，将蓝色圆圈的内容修改为Axes，就可以看到主图所示

++1.可以查看一下目前发布的话题，新开一个Terminal，运行如下命令

rostopic list

++2.可以查看话题发布与订阅的信息，新开一个Terminal，运行如下命令

rosrun rqt_graph rqt_graph