【SLAM】使用ROS的camera_calibration工具标定D435相机

本文介绍了如何使用ROS的camera_calibration工具标定D435相机，包括标定工具的安装和标定的步骤。阅读本文之前，请先参考博客安装ROS环境。

本文参考资料：

主要参考博客：利用ROS的Camera Calibration工具进行相机标定(USB相机与D435i)；
ROS官网：wiki.ros.org/action/fullsearch/camera_calibration；
可选，使用intel官方Depth Quality Tool进行标定：realsense D435 标定(calibration)_d435标定-CSDN博客；

安装标定工具

使用如下命令安装camera_calibration工具

1
2
3

sudo apt-get install -y \
    ros-melodic-rostest \
    ros-melodic-camera-calibration

命令执行结果如下

king@ubuntu:~/slam/ORB_SLAM2$ sudo apt-get install -y \
>     ros-melodic-rostest \
>     ros-melodic-camera-calibration
Reading package lists... Done
Building dependency tree     
Reading state information... Done
ros-melodic-camera-calibration is already the newest version (1.15.2-1bionic.20221025.191201).
ros-melodic-rostest is already the newest version (1.14.13-1bionic.20221025.183909).
ros-melodic-rostest set to manually installed.
The following packages were automatically installed and are no longer required:
  fonts-liberation2 fonts-opensymbol gir1.2-goa-1.0 gir1.2-gst-plugins-base-1.0 gir1.2-gstreamer-1.0 gir1.2-gudev-1.0 gir1.2-snapd-1 gir1.2-udisks-2.0 grilo-plugins-0.3-base gstreamer1.0-gtk3 libcdr-0.1-1 libclucene-contribs1v5 libclucene-core1v5 libcmis-0.5-5v5 libcolamd2
  libdazzle-1.0-0 libe-book-0.1-1 libedataserverui-1.2-2 libeot0 libepubgen-0.1-1 libetonyek-0.1-1 libevent-2.1-6 libexiv2-14 libfreerdp-client2-2 libfreerdp2-2 libgc1c2 libgee-0.8-2 libgexiv2-2 libgom-1.0-0 libgpgmepp6 libgpod-common libgpod4 liblangtag-common liblangtag1
  liblirc-client0 liblua5.3-0 libmediaart-2.0-0 libmspub-0.1-1 libodfgen-0.1-1 libqqwing2v5 librevenge-0.0-0 libsgutils2-2 libssh-4 libsuitesparseconfig5 libvncclient1 libwinpr2-2 libxapian30 libxmlsec1-nss lp-solve media-player-info python3-mako python3-markupsafe
  syslinux syslinux-common syslinux-legacy usb-creator-common
Use 'sudo apt autoremove' to remove them.
0 upgraded, 0 newly installed, 0 to remove and 3 not upgraded.

准备棋盘格标定板

ROS camera_calibration工具基于OpenCV和张正友标定法对相机进行标定。张正友标定法核心工具就是棋盘格标定板。如果有条件，可以去淘宝购买标准的棋盘格标定板。一般情况下，直接用A4纸打印出标定板也足够使用。

A4纸格式标定板下载链接：A4尺寸张正友标定法专用棋盘格；

在线生成标定板：Camera Calibration Pattern Generator – calib.io；

启动D435和标定工具

启动命令

先链接D435，然后启动相机和标定工具（不需要额外启动roscore）。

# 终端a启动相机
roslaunch realsense2_camera rs_camera.launch
# 终端b启动标定工具（RGB相机）
rosrun camera_calibration cameracalibrator.py \
    --size 8x6 --square 0.025 \
    --no-service-check \
    image:=/camera/color/image_raw \
    camera:=/camera/color

命令执行结果如下

命令参数介绍

启动命令参数介绍：

--size 8x6代表棋盘格内部的角点数量（不是格子数量），棋盘格一般是9x7个格子，但是计算的时候只使用棋盘格的内部角点。
--square 0.025是每个棋盘格的边长（米），也就是2.5厘米。上文给出的A4棋盘格打印出来之后就是2.5厘米的，可以用尺子量一下验证一下。

命令最后的topic映射，我们需要映射成D435的RGB相机或者红外相机，分别对其进行标定。三个相机对应的topic如下：

1
2
3

/camera/color/image_raw          # RGB相机
/camera/infra1/image_rect_raw    # 左红外
/camera/infra2/image_rect_raw    # 右红外

映射image是获取相机拍摄的图像，映射camera:=/camera/color的作用是让ROS能找到我们相机的/camera/color/image_raw和/camera/color/camera_info。

注意，默认情况下是不存在infra红外相机的topic的，是因为红外的topic没有被打开广播。使用如下命令主动开启红外摄像头的topic广播。

1 2	roslaunch realsense2_camera rs_camera.launch \ enable_infra1:=true enable_infra2:=true

此时可以看到topic列表，里面包含了infra的topic

[root:/]# rostopic list
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/color/metadata
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/depth/image_rect_raw/compressed
/camera/depth/image_rect_raw/compressed/parameter_descriptions
/camera/depth/image_rect_raw/compressed/parameter_updates
/camera/depth/image_rect_raw/compressedDepth
/camera/depth/image_rect_raw/compressedDepth/parameter_descriptions
/camera/depth/image_rect_raw/compressedDepth/parameter_updates
/camera/depth/image_rect_raw/theora
/camera/depth/image_rect_raw/theora/parameter_descriptions
/camera/depth/image_rect_raw/theora/parameter_updates
/camera/depth/metadata
/camera/extrinsics/depth_to_color
/camera/extrinsics/depth_to_infra1
/camera/extrinsics/depth_to_infra2
/camera/infra1/camera_info
/camera/infra1/image_rect_raw
/camera/infra1/image_rect_raw/compressed
/camera/infra1/image_rect_raw/compressed/parameter_descriptions
/camera/infra1/image_rect_raw/compressed/parameter_updates
/camera/infra1/image_rect_raw/compressedDepth
/camera/infra1/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra1/image_rect_raw/compressedDepth/parameter_updates
/camera/infra1/image_rect_raw/theora
/camera/infra1/image_rect_raw/theora/parameter_descriptions
/camera/infra1/image_rect_raw/theora/parameter_updates
/camera/infra1/metadata
/camera/infra2/camera_info
/camera/infra2/image_rect_raw
/camera/infra2/image_rect_raw/compressed
/camera/infra2/image_rect_raw/compressed/parameter_descriptions
/camera/infra2/image_rect_raw/compressed/parameter_updates
/camera/infra2/image_rect_raw/compressedDepth
/camera/infra2/image_rect_raw/compressedDepth/parameter_descriptions
/camera/infra2/image_rect_raw/compressedDepth/parameter_updates
/camera/infra2/image_rect_raw/theora
/camera/infra2/image_rect_raw/theora/parameter_descriptions
/camera/infra2/image_rect_raw/theora/parameter_updates
/camera/infra2/metadata
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf
/tf_static
[root:/]#

因为ROS会主动查询相机的set_camera_info节点，如果相机不存在此节点就会报错。可以添加--no-service-check参数跳过检查。如果不加这个参数，就会出现“Service not found”的报错。

[root:/]# rosrun camera_calibration cameracalibrator.py \
>     --size 8x6 --square 0.025 \
>     image:=/camera/color/image_raw \
>     camera:=/camera/color
Waiting for service /camera/color/set_camera_info ...
Service not found

开始标定

标定步骤

启动了标定工具，且显示出摄像头的画面之后，将棋盘标定板放在平面上（一定要是平面），然后从不同方向移动摄像头拍摄标定板，ROS会自动拍摄多角度的标定板图片。

标定完成后，calibration按钮会变绿可供点击。点击calibration后，save和commit按钮会亮起，点击save按钮之后会将标定结果数据写入到/tmp目录下去。注意一定要将其cp挪出来，因为linux的tmp目录每次启动系统都会清理掉。

1	('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')

而commit按钮就是将我们的标定结果写入到相机的固件中，后续rostopic echo /camera/color/camera_info的时候就会输出此次的标定结果。

标定输出

标定过程中，终端输出如下

*** Added sample 34, p_x = 0.593, p_y = 0.624, p_size = 0.568, skew = 0.121
*** Added sample 35, p_x = 0.488, p_y = 0.598, p_size = 0.560, skew = 0.335
*** Added sample 36, p_x = 0.433, p_y = 0.486, p_size = 0.575, skew = 0.358
*** Added sample 37, p_x = 0.544, p_y = 0.345, p_size = 0.580, skew = 0.248
*** Added sample 38, p_x = 0.522, p_y = 0.179, p_size = 0.568, skew = 0.055
*** Added sample 39, p_x = 0.511, p_y = 0.019, p_size = 0.591, skew = 0.106
*** Added sample 40, p_x = 0.496, p_y = 0.547, p_size = 0.577, skew = 0.143
*** Added sample 41, p_x = 0.502, p_y = 0.344, p_size = 0.599, skew = 0.004

标定完成后会输出相机内参。其中camera matrix是相机的内参矩阵（对应开头矩阵K），distortion是相机的畸变系数（对应开头的矩阵D），R是旋转举证，P是投影矩阵。[narrow_stereo]之后的内容是opencv格式的ost.txt标定文件格式，可以用于发布camera_info话题

**** Calibrating ****
mono pinhole calibration...
D = [0.1234477524818914, -0.24186083107572148, 0.010573738134724179, -0.0018947635408389889, 0.0]
K = [609.8463473151232, 0.0, 318.16312952971543, 0.0, 610.5650055668543, 255.8048346565674, 0.0, 0.0, 1.0]
R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P = [621.8898315429688, 0.0, 317.1112464092075, 0.0, 0.0, 620.319091796875, 259.4397678761525, 0.0, 0.0, 0.0, 1.0, 0.0]
None
# oST version 5.0 parameters


[image]

width
640

height
480

[narrow_stereo]

camera matrix
609.846347 0.000000 318.163130
0.000000 610.565006 255.804835
0.000000 0.000000 1.000000

distortion
0.123448 -0.241861 0.010574 -0.001895 0.000000

rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000

projection
621.889832 0.000000 317.111246 0.000000
0.000000 620.319092 259.439768 0.000000
0.000000 0.000000 1.000000 0.000000

('Wrote calibration data to', '/tmp/calibrationdata.tar.gz')