Ubuntu 18.04 Intel RealSense D435i 相机标定教程

1、D435i相机简介

RealSenseD435i 是一款立体视觉深度相机，如下图所示，其集成了两个红外传感器（IR Stereo Camera）、一个红外激光发射器（IR Projector）和一个彩色相机(Color Camera)。立体深度相机系统主要包括两部分：视觉处理器D4和深度模块。主机处理器连接 USB 2.0 / USB 3.1 Gen 1、视觉处理器 D4 位于主处理器主板上，RGB颜色传感器数据通过主处理器主板和 D4 板上的彩色图像信号处理器（ISP）发送到视觉处理器D4。IntelRealSense D435i提供了完整的深度相机模块，集成了视觉处理器、立体深度模块、RGB 传感器以及彩色图像信号处理模块。深度模块采用立体视觉的左右成像器、可选的红外激光发射器和 RGB 色彩传感器。

2、安装和编译标定工具

2.1 下载编译code_utils

创建ros工作空间，用于code_utils以及后面的imu_utils，工作空间名和路径可以自己更改，code_utils和imu_utils都是imu标定需要用到的，用于标定imu噪声密度以及随机游走系数

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
source ~/imu_catkin_ws/devel/setup.bash
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

如果有报错sumpixel_test.cpp:2:10: fatal error: backward.hpp: 没有那个文件或目录，将sumpixel_test.cpp中# include "backward.hpp"改为：#include "code_utils/backward.hpp"。

2.2 下载编译imu_utils

cd ~/imu_catkin_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

2.3 安装kalibr

2.3.1 按照官方教程安装kalibr(https://github.com/ethz-asl/kalibr/wiki/installation)

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-module
sudo apt-get install software-properties-common software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython
sudo apt-get install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo apt-get install python-igraph

2.3.2 创建工作空间

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

2.3.3 下载源码

cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git

2.3.4 编译源码

cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source ~/kalibr_workspace/devel/setup.bash

3、IMU标定

3.1 写标定参数文件

找到realsense-ros包，进入/catkin_ws/src/realsense2_camera/launch（路径仅供参考），复制其中的rs_camera.launch，并重命名为rs_imu_calibration.launch(命名随意)，并对里面的内容做如下更改

<arg name="unite_imu_method"          default=""/>
//       ########### 改为#############
<arg name="unite_imu_method"          default="linear_interpolation"/>
<arg name="enable_gyro"         default="false"/>  “false” 改为”true”
<arg name="enable_accel"        default="false"/>   “false” 改为”true”

这样做的目的是将accel和gyro的数据合并得到imu话题，如果不这样做发布的topic中只有加速度计和陀螺仪分开的topic，没有合并的camera/imu 话题。

3.2 编写启动文件

打开/imu_catkin_ws/src/imu_utils/launch（路径仅供参考），打开终端运行以下命令。

gedit d435i_imu_calibration.launch  （文件名称可以自定义，在文件中写入以下内容）

<launch><node pkg="imu_utils" type="imu_an" name="imu_an" output="screen"><!--TOPIC名称和上面一致--><param name="imu_topic" type="string" value= "/camera/imu"/><!--imu_name 无所谓--><param name="imu_name" type="string" value= "d435i"/><!--标定结果存放路径--><param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/><!--数据录制时间-min 120分钟 可以自行修改 一般要大于60--><param name="max_time_min" type="int" value= "120"/><!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为400，为后面的rosbag play播放频率--><param name="max_cluster" type="int" value= "400"/></node>
</launch>

3.3 录制imu数据包

插上相机，realsense静止放置，放置时间要稍大于d435i_imu_calibration.launch中的录制时间，即大于120分钟。

roscore
roslaunch realsense2_camera rs_imu_calibration.launch
cd ~/imu_catkin_ws      //等下录制到这个文件夹上
rosbag record -O imu_calibration /camera/imu    // 生成的包名称imu_calibration.bag

等待两小时录制完之后就按下ctrl+c，结束录制，注意在录制的前1个小时imu_calibration.bag文件很可能大小一直都是4.1KB，1个小时后会突然增大，所以要耐心等待，中途不要放弃，我在这里卡了很久。

你会发现当前目录有一个名为 imu_calibration.bag的文件，其中imu_calibration是bag包的名字，可以更改，/camera/imu是发布的IMU topic，可以通过以下命令查看。

rostopic list -v

3.4 运行校准程序

首先激活imu_util工作空间的setup.bash，前面路径仅供参考。

source ~/imu_catkin_ws/devel/setup.bash
roslaunch imu_utils d435i_imu_calibration.launch

3.5 回放数据包

  打开新的终端，cd 存放imu_calibration.bag的路径。
source ~/imu_catkin_ws/devel/setup.sh
cd ~/imu_catkin_ws //数据包在这个文件夹下
rosbag play -r 400 imu_calibration.bag  其中 -r 400是指400速播放bag数据

3.6 标定结果

标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件，其中d435i_imu_param.yaml就是我们想要的结果，如下图所示。

自己的标定结果如下

%YAML:1.0
---
type: IMU
name: d435i
Gyr:unit: " rad/s"avg-axis:gyr_n: 2.4374348304191541e-02gyr_w: 4.1351964638005760e-04x-axis:gyr_n: 3.4349742772545717e-02gyr_w: 6.6400202524599536e-05y-axis:gyr_n: 1.8369698876417567e-02gyr_w: 5.7877885322928221e-04z-axis:gyr_n: 2.0403603263611333e-02gyr_w: 5.9537988338629125e-04
Acc:unit: " m/s^2"avg-axis:acc_n: 4.1009844816965006e-02acc_w: 4.8708380066138992e-04x-axis:acc_n: 3.2801162643496862e-02acc_w: 2.8388101885379135e-04y-axis:acc_n: 3.7724057308126648e-02acc_w: 4.6927333080848470e-04z-axis:acc_n: 5.2504314499271502e-02acc_w: 7.0809705232189376e-04

realsense自带的参数通过以下命令获得：

roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/accel/imu_info
rostopic echo /camera/gyro/imu_info

4、双目视觉标定

4.1 下载标定板打印出来

到https://github.com/ethz-asl/kalibr/wiki/downloads选择其中一个

4.2 自己生成标定板

这种方式更灵活，而且可以将生成的标定板直接在电脑里面打开，作为目标进行标定。

cd ~/kalibr_workspace   //你的工程路径
source ./devel/setup.sh
kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3  // 生成PDF文件
不论是打印PDF标定还是直接在电脑里面打开PDF标定，都要实际测量一下二维码方格和小方格的的长度，再填到yaml文件里面。
--type apriltag                标定板类型
--nx [NUM_COLS]                列个数
--ny [NUM_ROWS]                行个数
--tsize [TAG_WIDTH_M]          二维码方格长度，单位m
--tspace [TAG_SPACING_PERCENT] 小方格与二维码方格长度比例

新建april_6x6_A4.yaml文件，格式参考上面的yaml，内容展示如下：

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.022           #size of apriltag, edge to edge [m]  要亲自拿尺子量一下
tagSpacing: 0.3          #ratio of space between tags to tagSize

4.3 关闭结构光

默认开始结构光时，双目图像会有很多点，这些点可能对标定有影响，所以使用时需要关闭结构光。

roslaunch realsense2_camera rs_camera.launch//注意不是：rs_imu_calibration.launch
新打开终端，运行
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled对钩去掉，展示如下：

4.4 确定相机到标定板的大致距离

打开rviz观察合理的距离，要求摄像头能看到RGB/左目/右目标定棋盘格。在终端输入以下命令。

roslaunch realsense2_camera rs_camera.launch
rviz

打卡rviz之后，注意一定要先在左侧 Fixed Frame 选择camera_link。左下角 add --> By topic --> /camera/color/image_raw/ --> 双击Camera ，找一个适合的能拍到棋盘格的距离。用同样的方式添加双目topic，/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw。注意：如果找不到双目对应的topic，则需要找到realsense2_camera rs_camera.launch文件进行修改！！！都改为true。

  <arg name="enable_infra"        default="true"/><arg name="enable_infra1"       default="true"/><arg name="enable_infra2"       default="true"/>

4.5 修改相机帧数（官方推荐是4Hz，尽管实际频率不完全准确，但是不影响结果）

kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1对应左目相机，infra2对应右目相机。

在终端输入以下指令：

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color & rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right

4.6 录制ROS数据包

在终端输入以下命令

rosbag record -O multicameras_calibration /infra_left /infra_right /color

录制包的时候有以下注意事项：盯着rviz里面的画面，确保RGB、左目、右目都能看到完整的棋盘格；前后，左右，移动和小幅度、缓慢地翻转相机，持续2分钟左右，移动的时候，确保相机可以清晰完整地拍到棋盘格。如下图所示，我是直接在电脑里面打开PDF标定文件进行标定的。

查看文件属性信息

rosbag info multicameras_calibration.bag

最后录制的包大小有12.9G，所以一定要空出足够的硬盘空间。

4.7 使用Kalibr标定

source devel/setup.bash

Kalibr和realsense可能不在同一个工作空间，所以首先要激活环境变量，切换到kalibr的工作空间。然后在终端运行以下命令。

rosrun kalibr kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag  multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction

其中–target …/yaml/april_6x6_A4.yaml是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。–bag multicameras_calibration.bag是录制的数据包，models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）， --topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题，–bag-from-to 10 100表示处理bag中10-100秒的数据。–show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。

标定过程如下图所示

最后生成的文件如下图所示。.yaml 主要作用为了后期IMU+相机联合标定；.pdf 以图的方式显示效果；.txt 含有相机的内参。

4.8 相机标定命令总结

//终端1
roscore
//终端2
roslaunch realsense2_camera rs_camera.launch
//终端3
rviz
//终端4
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color & rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
//终端5
rosbag record -O multicameras_calibration /infra_left /infra_right /color
终端6
source devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --target april_6x6_A4.yaml --bag  multicameras_calibration.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction

5、imu+双目标定

5.1 编辑yaml文件

新建 camchain.yaml，imu.yaml。命令gedit camchain.yaml。参数参考上面得到的multicameras_calibration-camchain.yaml文件，没有的参数可以删除，最终结果示例如下：

cam0:camera_model: pinholedistortion_coeffs: [0.540465033980622, -3.421102632272967, 21.830789418316407, -43.498448598565396]distortion_model: equidistantintrinsics: [438.8503955423126, 439.4255541557132, 427.72752812368054, 245.94481053159686]resolution: [848, 480]rostopic: /infra_left
cam1:T_cn_cnm1:- [0.9998501766643103, -0.000373712809663085, -0.017305622296793482, -0.04586677535366495]- [0.00041133383503570486, 0.9999975600577051, 0.002170410815566145, 0.0001142871368050811]- [0.01730476896174973, -0.0021772040253649635, 0.999847890808301, 3.897799547306375e-05]- [0.0, 0.0, 0.0, 1.0]camera_model: pinholedistortion_coeffs: [-0.19909517915337813, 5.176635647931839, -20.70781432752059, 30.426962394390223]distortion_model: equidistantintrinsics: [449.13192407757515, 449.84988819302635, 431.79225642158184, 243.65260464616762]resolution: [848, 480]rostopic: /infra_right

命令 gedit imu.yaml，参数使用之前得到的d435i_imu_param.yaml，示例如下：

#Accelerometers
accelerometer_noise_density: 2.0546933436785896e-02   #Noise density (continuous-time)  acc_n
accelerometer_random_walk:   1.0213093603518382e-03   #Bias random walk   acc_w
#Gyroscopes
gyroscope_noise_density:     1.9035698029406516e-03   #Noise density (continuous-time)  gyr_n
gyroscope_random_walk:       1.9530927686562762e-05   #Bias random walk  gyr_w
rostopic:                    /imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

5.2 新建rs_imu_stereo.launch

复制realsense-ros包中rs_camera.launch，重命名为rs_imu_stereo.launch，更改内容为

<arg name="enable_sync"               default="false"/>
//改为：
<arg name="enable_sync"               default="true"/>
这样来使imu和双目数据时间对齐
<arg name="unite_imu_method"          default=""/>
//改为
<arg name="unite_imu_method"         default="linear_interpolation"/>
这样来保证会有imu话题

5.3 同样关闭结构光，参考上面。

5.4 调整频率

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right & rosrun topic_tools throttle messages /camera/imu 200.0 /imu
rostopic hz topic名可以查看实际频率

5.5 录包（录包注意事项和前面一致）

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

5.6 标定

source  ~/kalibr_workspace/devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --bag  imu_stereo.bag --cam  camchain.yaml --imu imu.yaml --target april_6x6_A4.yaml --bag-from-to 15 115 --show-extraction

标定过程如下图所示

标定结果如下图所示