使用apriltag_ros检测相机姿态

0 环境

• Nvidia Jetson Orin NX
• Jetpack 5.1.1
• 双目相机
• ROS noetic

1 相机驱动

1.1 建立ROS工作空间

建立工作空间

# 在home下名为catkin_ws的新建文件夹
mkdir -p  ~/catkin_ws/src

# 进入catkin_ws文件夹
cd ~/catkin_ws/src

# 将当前文件夹属性转化为工作空间
catkin_init_workspace
1
2
3
4
5
6
7
8

编译工作空间

# 进入catkin_ws文件夹
cd ~/catkin_ws/

# 或使用返回上一级
cd ..

# 编译文件夹
catkin_make
catkin_make install
1
2
3
4
5
6
7
8
9

设置环境变量

gedit ~/.bashrc
1

在其中加入

# 每个人路径不同
source /opt/ros/noetic/setup.bash
source /home/work/catkin_ws/devel/setup.bash
1
2
3

1.2 安装usb_cam包

cd ~/catkin_ws/src
https://github.com/ros-drivers/usb_cam.git
cd ~/catkin_ws
catkin_make
1
2
3
4

1.3 配置usb_cam参数

cd ~/catkin_ws/src/usb_cam/config
gedit usb_cam.yml
1
2

需要修改的参数包括

video_device 相机的端口号
pixel_format 相机输出格式
image_width 长度
image_height 宽度
framerate 帧率
1
2
3
4
5

1.4 如何查看相机的分辨率和帧率

# 安装v4l工具
sudo apt install v4l-utils

# 列出所有设备
v4l2-ctl --list-devices

# 列出指定设备的预览支持格式
v4l2-ctl --list-formats-ext --device /dev/video0
1
2
3
4
5
6
7
8

其他用法请参考
https://blog.csdn.net/u012906122/article/details/126356698
https://www.mankier.com/1/v4l2-ctl

1.5 运行usb_cam

roslaunch usb_cam usb_cam-test.launch
1

打开rqt查看图像

rqt_image_view
1

2 相机标定

2.1 单目相机的标定

# 打开usb_cam
roslaunch usb_cam usb_cam-test.launch
# 开始标定，size是棋盘格数量，square是单个格子大小
rosrun camera_calibration cameracalibrator.py --size 6x7 --square 0.108 image:=/usb_cam/image_raw camera:=/usb_cam
1
2
3
4

打开标定界面后，请在视野中移动、倾斜标定板，直至左上角四个进度条都是绿色，此时CALIBRATE亮起，点击开始计算

之后点击SAVE保存，点击COMMIT退出

在保存路径中提取ost.yaml，放置在~/catkin_ws/src/usb_cam，修改usb_cam-test.launch

<launch>
  <arg name="image_view" default="false" />

  <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
      <rosparam command="load" file="$(find usb_cam)/config/usb_cam.yml"/>
      <param name="camera_info_url" type="string" value="file:///home/orin511/catkin_ws/src/usb_cam/ost.yaml" />
  </node>
  <node if="$(arg image_view)" name="image_view" pkg="image_view" type="image_view"
        respawn="false" output="screen">
    <remap from="image" to="/usb_cam/image_raw"/>
    <param name="autosize" value="true" />
  </node>
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13

2.2 双目相机的标定

暂无

3 使用april_tag包

3.1 安装apriltag包

sudo apt install apriltag
1

或者编译安装（不要放在ROS工作空间里）

cd ~
git clone https://github.com/AprilRobotics/apriltag.git
cd apriltag
mkdir build
cd build
cmake ..  
make
sudo make install  #安装到ros中
1
2
3
4
5
6
7
8

3.2 安装apriltag_ros包

cd ~/catkin_ws/src
git clone https://github.com/AprilRobotics/apriltag_ros.git 
cd ~/catkin_ws
catkin_make
1
2
3
4

3.3 配置apriltag_ros包

配置launch文件

gedit continuous_detection.launch
1

修改camera_name为/usb_cam
修改image_topic为image_raw

>
  <!-- set to value="gdbserver localhost:10000" for remote debugging -->
  

  <!-- configure camera input -->
  
  
  

  <!-- apriltag_ros continuous detection node -->
  
    
    
    

    id: 0, size: 0.09, name: Lup},
    {id: 1, size: 0.09, name: Rup},
    {id: 2, size: 0.09, name: Ldown},
    {id: 3, size: 0.09, name: Rdown},
  ]
# ## Tag bundle definitions
# ### Remarks
#
# - name is optional
# - x, y, z have default values of 0 thus they are optional
# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
#
# ### Syntax
#
# tag_bundles:
#   [
#     {
#       name: 'CUSTOM_BUNDLE_NAME',
#       layout:
#         [
#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
#           ...
#         ]
#     },
#     ...
#   ]
tag_bundles:
  [
  ]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

3.4 使用apriltag_ros包

roslaunch usb_cam usb_cam-test.launch
roslaunch apriltag_ros continuous_detection.launch
1
2

使用RVIZ查看，需要视野内有tag，而且将坐标系选为相机

rviz
1

选择TF坐标系即可观察

3.5 Bug修复

在Rviz中有概率无法使用TF或image观察结果，一般表现为画面中出现tag时会卡死，这一问题目前无法解决，但可以进行缓解

https://github.com/AprilRobotics/apriltag_ros/issues/153

注释掉 continuous_detector.cpp 中的以下行可以缓解此问题，但不会发布 /tag_detections_image 主题。/tag_detections 和 /tf 主题工作正常。

if (draw_tag_detections_image_)
  {
    tag_detector_->drawDetections(cv_image_);
    tag_detections_image_publisher_.publish(cv_image_->toImageMsg());
  }
1
2
3
4
5