Realsense D435i RGB+IMU标定

准备工作：

Ubuntu
ros环境
librealsense
realsense-ros
本次标定使用两个工具，一个是kalibr工具，可以标定相机内参以及相机-IMU外参，另一个是港科大出品的imu_utils，可以标定IMU内参，我们先标定IMU内参，再标定相机内参，最后联合标定，得到相机-IMU的外参。

一、IMU标定

1.修改rs_camera.launch部分参数

使得IMU数据同步（针对vins_mono运行IMU时间戳混乱的情况imu message ins disorder）
具体步骤：
找到realsense-ros工作环境文件夹中的rs_camera.launch
位置：在 /realsense的catkin工作环境/src/realsense-ros/realsense2_camera/launch/
把rs_camera.launch中的

  <arg name="unite_imu_method"      default="copy"/>

中的 “copy” 修改成"linear_interpolation"

分别打开三个terminal，电脑插上realsense D435i，试着运行以下：

roscore 
roslaunch realsense2_camera rs_camera.launch
rostopic list 

在这个第三个terminal下查看是否有 /camera/imu ，就这个的话说明ok

2.配置港科大IMU标定程序的环境

(1)安装ceres

去官网下载源码编译就好了。
https://github.com/ceres-solver/ceres-solver
http://www.ceres-solver.org/

(2)创建ros工作空间

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
catkin_init_workspace
cd ~/imu_catkin_ws 
catkin_make
source ~/imu_catkin_ws/devel/setup.bash

(3)下载code_utils并编译

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

(4) 下载imu_utils并编译

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

3.写标定参数文件

cd ~/imu_catkin_ws/src/imu_utils/launch
touch d435i_imu_calibration.launch
gedit d435i_imu_calibration.launch

文件格式，可直接复制

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <param name="imu_name" type="string" value= "d435i_imu_calibration"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "10"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

需要注意的是max_time_min代表的是标定时间，这里的单位是分钟，意思是填10就是代表10分钟，至少录十分钟。

4.录制IMU数据包

插上相机

roscore 
roslaunch realsense2_camera rs_camera.launch

让相机静止，不要动它

cd ~/imu_catkin_ws      //等下录制到这个文件夹上
rosbag record -O imu_calibration /camera/imu 

至少录制你刚刚设定的时间以上(max_time_min)
比如上面设定10，这里就要录足十分钟
录制完之后就按下ctrl+c，结束录制
你会发现当前目录有一个名为 imu_calibration.bag的文件

5.使用imu标定工具进行标定

(1)打开一个terminal

source ~/imu_catkin_ws/devel/setup.sh 
rosbag imu_utils d435i_imu_calibration.launch 

(2)新打开一个terminal

source ~/imu_catkin_ws/devel/setup.sh 
cd ~/imu_catkin_ws //数据包在这个文件夹下
rosbag play -r 200 imu_calibration.bag

标定完之后，看(1)的terminal，会有一串东西产生，而且该terminal已经可以输入命令了（意味着标定进程已经结束），说明已经标定好了，这时候要看结果

cd ~/imu_catkin_ws/src/imu_utils/data 

里面有一个文件：
d435i_imu_calibration_imu_param.yaml
就是你想要的IMU标定结果啦

%YAML:1.0
---
type: IMU
name: d435i_imu_calibration
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
   x-axis:
      gyr_n: 4.5370714417877871e-03
      gyr_w: 4.0841410937003261e-05
   y-axis:
      gyr_n: 3.3866604818486266e-03
      gyr_w: 3.8353526280536766e-05
   z-axis:
      gyr_n: 1.6224695149202378e-03
      gyr_w: 1.2885257092657719e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04
   x-axis:
      acc_n: 1.9864483668885411e-02
      acc_w: 5.5777874344305667e-04
   y-axis:
      acc_n: 2.3746240477749841e-02
      acc_w: 5.0079781850747836e-04
   z-axis:
      acc_n: 3.5737875629012449e-02
      acc_w: 1.1047807619580928e-03

我们等下只用到其中四个参数：

Gyr:
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
Acc:
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04

分别是陀螺仪和加速度计 随机游走和 高斯白噪声的平均值，是IMU噪声模型中的两种噪声。

二、使用kalibr标定相机

1.安装Kalibr

建议源码安装
https://github.com/ethz-asl/kalibr/wiki/installation

有一点不同的就是他是ubuntu14的环境，如果你ubuntu的版本不同， 你需要把下面关于ROS的包都换成你自己的版本。

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

比如ros-indigo-image-transport-plugins
我的是Ubuntu 18，装的是ROS Melodic版本，换成：
ros-melodic-image-transport-plugins
即可

2.下载&打印标定板

https://github.com/ethz-asl/kalibr/wiki/downloads
这里推荐的是 Aprilgrid 标定板。

https://github.com/ethz-asl/kalibr/wiki/calibration-targets
这里有三个参数的说明，标定板格子大小之类的。

我下载的是Aprilgrid 6x6 0.5x0.5 m (unscaled)
使用adobe acrobat pro dc 软件自定义缩放40%打印，A4纸刚好能打印出来 。

原版的参数是：
6X6 tags 6乘6个格子
一个大格子size=5.5cm
一个小格子spacing=1.65cm

40%的缩放：
6X6 tags
一个大格子size=2.2cm
一个小格子spacing=0.66cm
记得打印出来用尺子量一下，以免出现差错。
下载官网提供的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

然后把打印的标定纸贴在墙上

3.确定适合距离

打开rviz观察合理的距离，要求摄像头能看到标定棋盘格。

roscore 
roslaunch realsense2_camera rs_camera.launch
rviz

没有安装rviz就自行安装一下

sudo apt-get install rviz

左侧 Fixed Frame 选择camera_link
左下角 add --> By topic --> /camera/color/image_raw/ --> 双击Camera ，找一个适合的能拍到棋盘格的距离
关闭

4.修改相机帧数

修改相机帧数到4hz(要求图像频率不可过高)

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color

注意这里是采用了新的话题去发布：/color
所以下面录制要写/color话题

5.录制ROS数据包

rosbag record -O camd435i /color

这里没说录多少时间，录个一分钟就好了，录的时候对着标定板移动
按下ctrl+c 结束录制
看到当前文件夹会出现camd435i.bag数据包

6.使用Kalibr进行标定

在刚刚建立的ROS中的Kalibr的工作空间，执行source devel/setup.sh
执行：
注意：请自行修改文件名与位置

kalibr_calibrate_cameras --target /位置/文件名.yaml --bag /位置/camd435i.bag --bag-from-to 26 100 --models pinhole-radtan --topics /color --show-extraction

26-100 指的是录制的第26秒到100秒这段时间
pinhole-radtan指的是针孔相机模型和畸变模型

标定完之后就可以发现当前目录下出现了三个文件，就是标定的结果。
camchain-…camd435i.yaml
results-cam-…camd435i.txt
report-cam-…camd435i.pdf

我们看看生成的yaml文件：

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.11114194921960645, -0.2171600270591326, -0.0037942616908056637,
    -0.004000340562604045]
  distortion_model: radtan
  intrinsics: [579.5589935482029, 578.1762234104125, 326.6772381912598, 243.3955018655041]
  resolution: [640, 480]
  rostopic: /color

intrinsic 是内参
distortion_coeffs 是畸变参数

三、camera_imu联合标定

标定完camera，IMU，得到相应的yaml文件（即分别为相机和IMU的参数）。
官方教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

1.调节帧率

相机20Hz，IMU200Hz，官方说这个参数比较好，并分别以/color和/imu为话题名发布

rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

2.录制数据包

官方似乎没有说录多久，但是说了每一个方向都要给激励，可以参考官网的视频。YouTube看不了的话可以看泡泡机器人搬运过来的视频。
链接在此
先把目录移动到刚刚建立的kalibr工作环境目录下。
然后执行source devel/setup.sh

rosbag record -O dynamic /color /imu

3.重写yaml文件

标定需要三个文件，一个是相机的标定文件，一个是IMU的标定文件，一个是录制的数据包

(1)相机标定的yaml文件

由于我们刚刚也是在kalibr工作环境下标定的，所以直接用刚刚标定好的相机yaml文件即可。
即上述所说的
camchain-…camd435i.yaml

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.11114194921960645, -0.2171600270591326, -0.0037942616908056637,
    -0.004000340562604045]
  distortion_model: radtan
  intrinsics: [579.5589935482029, 578.1762234104125, 326.6772381912598, 243.3955018655041]
  resolution: [640, 480]
  rostopic: /color

(2)新建imu.yaml

在kalibr工作环境目录下新建imu.yaml文件，格式如下：
格式如下：

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

rostopic:                    /imu0      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

已知

Gyr:
   avg-axis:
      gyr_n: 3.1820671461855504e-03
      gyr_w: 3.0693398103399251e-05
Acc:
   avg-axis:
      acc_n: 2.6449533258549235e-02
      acc_w: 7.2111910796954259e-04

把IMU标定结果对应地填到imu.yaml去，如下：

#Accelerometers
accelerometer_noise_density: 2.64e-02   #Noise density (continuous-time)
accelerometer_random_walk:   7.21e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     3.18e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.07e-05   #Bias random walk

rostopic:                    /imu   #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

4.使用Kalibr进行标定

别忘了先在kalibr工作目录下执行 source devel/setup.sh
然后下面的参数仍然需要自己修改

kalibr_calibrate_imu_camera --target 标定板位置/标定板名称.yaml --cam 相机标定位置/相机标定名称.yaml --imu imu.yaml --bag dynamic.bag --show-extraction

运行时间会根据你录制数据包的大小和电脑性能而定，我录制了将近两分钟，电脑是四年前的神舟，所以速度较慢，大概用了有半个小时以上，可以去喝杯咖啡再回来看看。

生成好多个文件，主要关注一个文件：

camchain-imucam-dynamic.yaml

里面有一个T_cam_imu，就是我们要求的外参。

四、总结

前前后后搞了两天，真的多亏了各位大佬的博客，才能完成这次的标定，Kalibr的官方教程对新手也太不友好了…然而我也写的不是很详细，有什么问题可以在下面留言。下次我们来标定D435i的剩下的双目摄像头，敬请期待。

参考：

https://blog.csdn.net/qq_41839222/article/details/86552367
https://blog.csdn.net/qq_25458977/article/details/102806768
https://blog.csdn.net/fang794735225/article/details/92804030
https://blog.csdn.net/qq_36170626/article/details/102998942?utm_source=app
https://blog.csdn.net/kongdaqing1290/article/details/97524087
https://blog.csdn.net/weixin_40628128/article/details/95945945?utm_source=app
https://blog.csdn.net/qq_36170626/article/details/102998942?utm_source=app