Ubuntu16.04下使用kalibr标定intel RealSense D435i imu+双目

使用kalibr标定RealSense D435i imu+双目

一、实验背景

• RealSense D435i包含了rgb图像、左视图、右视图、深度图、imu等主要的数据。为了更好的使用这些数据进行算法的验证，首先要做的就是对这些数据对应的传感器进行标定。
• 本文主要针对 双目+imu 涉及的传感器进行标定，其他可以同理参考。

二、系统配置

• 系统环境：Ubuntu16.04
• ROS版本：ros-kinetic

三、环境准备

1. RealSense 驱动安装

• 可参考之前的博客【Ubuntu16.04安装intel RealSense D435i驱动并在ROS中使用】，最终打开realsense-viewer后可以看到深度图、rgb图、imu数据如下图所示：
  
  注意：相机的usb线要使用官方原装的3.0的线，电脑的usb接口也要使用3.0的接口，否则可能会因为数据传输过慢而出现无法实时传输，画面卡住等问题。

2. RealSense ROS 包安装

• 可参考之前的博客【Ubuntu16.04安装intel RealSense D435i驱动并在ROS中使用】，最终实现结果为终端打开rviz，Fixed Frame 选择camera_link，通过Add添加topic：/camera/accel/sample、/camera/gyro/sample、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw后可以看到如下画面（说明可以成功读取到加速计、陀螺仪、双目数据）：
  
  注意：陀螺仪的数据在rviz中是无法可视化的，只能可视化加速计的数据。

3. Ceres安装

• 可参考官方教程：http://www.ceres-solver.org/installation.html
• 由于Ceres v2.0版本需要C++14，Ceres <= v1.14版本需要C++11，所以我安装的是Ceres v1.14，github链接为：https://github.com/ceres-solver/ceres-solver/tree/1.14.0

4. 下载编译code_utils

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

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

• 下载编译code_utils:

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

可能出现以下错误：

1. catkin_make时出现libdw.h没有找到：
   
   解决方法：

sudo apt-get install libdw-dev

2. catkin_make时出现backward.hpp没有找到：
   
   解决方法：
   将 sumpixel_test.cpp 中的 #include "backward.hpp" 改为：#include "code_utils/backward.hpp"。

5. 下载编译imu_utils

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

6. 安装Kalibr

• 可参考官方教程：https://github.com/ethz-asl/kalibr/wiki/installation，建议用源码安装。

四、开始标定

1. imu标定

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

1. 将
   <arg name="unite_imu_method" default=""/>
   改为：
   <arg name="unite_imu_method" default="linear_interpolation"/>
   这样做的目的是将accel和gyro的数据合并得到imu话题。如果不这样做，发布的topic中只有加速计和陀螺仪分开的topic，没有合并的/camera/imu topic。
   然后运行启动文件：

roslaunch realsense2_camera rs_imu_calibration.launch

• 步骤二：编写启动文件，打开/Code/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-->
        <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>

• 步骤三：录制imu数据包。realsense静止放置，放置时间要稍大于d435i_imu_calibration.launch中的录制时间，即大于120分钟：

rosbag record -o imu_calibration /camera/imu

其中imu_calibration是bag包的名字，可以更改；/camera/imu是发布的IMU topic，可以通过以下命令查看：

rostopic list

• 步骤四：运行校准程序：

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

• 步骤五：回放数据包，打开新的终端：

cd 存放imu_calibration_2020-10-28-14-51-09.bag的路径
rosbag play -r 400 imu_calibration.bag

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

作为对比，realsense自带的参数都是0：

2. 多相机标定

• 步骤一：下载打印标定板，到https://github.com/ethz-asl/kalibr/wiki/downloads选择Aprilgrid 6x6 0.5x0.5 m (unscaled)：
  
  
  下载，然后缩放到40%，用A4纸就可以打印出来：

原始pdf的格子参数是：
6*6的格子
大格子边长：5.5cm
小格子边长：1.65cm
小格子与大格子边长比例：0.3

调整后的格子参数是：
大格子边长：2.2cm
小格子边长：0.66cm
小格子与大格子边长比例：0.3

但这只是理想情况，实际情况还得实际测量。
新建april_6x6_50x50cm_A4.yaml文件，格式参考上图标定板对应的yaml文件，内容展示如下：

注意：一定要自己测量大格子边长，即tagSize。我测出来实际上是0.024，所以我的文件里面tagSize是0.024，请自行更改。

• 步骤二：启动关闭结构光。默认是开启结构光的，双目图像会有很多光斑，这些点可能对标定有影响，所以使用时需要关闭结构光。
  先启动：

 roslaunch realsense2_camera rs_camera.launch

再开一个新的终端，运行：

rosrun rqt_reconfigure rqt_reconfigure

打开后将camera->stereo_module中的emitter_enabled取消勾选，展示如下：

• 步骤三：确定realsense合适的放置位置。新打开终端，运行rviz，之后在里面add rgb和双目对应的topic，/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw，展示如下：
  
  之后对准标定板，尝试移动realsense，同时要确保标定板一直在三个图像当中。录制过程参考：https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop
• 步骤四：修改相机帧数（官方推荐是4Hz，尽管实际频率不完全准确，但是不影响结果）。kalibr在处理标定数据的时候要求频率不能太高，一般为4Hz，我们可以使用如下命令来更改topic的频率，实际上是将原来的topic以新的频率转成新的topic，实际测试infra1才是对应左目相机。新开终端，运行：

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

注意：这种方式可能导致不同摄像头的时间不同步，如果出现这个问题，可以尝试不做这个操作，不这样做意味着需要更多的处理时间，这样的话后面也要相应的更改。（但是我在实验过程中没有出现这个问题）

• 步骤五：录制ROS数据包，运行以下命令：

rosbag record -o multicameras_calibration /infra_left /infra_right /color

后面三个topic就是转换频率后新发出的topic。

• 步骤六：使用Kalibr标定。运行以下命令：

source devel/setup.bash
kalibr_calibrate_cameras --target ../Aprilgrid/april_6x6_50x50cm_A4.yaml --bag  ../multicameras_calibration_2020-10-29-20-19-06.bag --models pinhole-equi pinhole-equi pinhole-equi --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction

其中：

1. --target ../Aprilgrid/april_6x6_50x50cm_A4.yaml是标定板的配置文件，如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量。
2. --bag ../multicameras_calibration_2020-10-29-20-19-06.bag是录制的数据包；
3. --models pinhole-equi pinhole-equi pinhole-equi表示三个摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）；
4. --topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题；
5. –bag-from-to 10 100表示处理bag中10-100秒的数据。（我在实验过程中没有加–bag-from-to 10 100，所以处理的是bag里所有的数据，标定时间比较长）
6. –show-extraction表示显示检测特征点的过程。
   这些参数可以相应的调整。
   这个过程可能出现一些报错，问题及解决方法参考：https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2_allsobaiduend~default-4-105646064.nonecase&utm_term=d435%20realsense%20%E5%8F%8C%E7%9B%AE&spm=1000.2123.3001.4430。
   （因为我在做的时候没有出现问题，所以不再展开描述。）
   最终标定完成后在Kalibr的工程目录下得到三个文件：
   

3. imu+双目标定

• 步骤一：编写camchain.yaml，格式参考Kalibr官方教程https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的chain.yaml，具体的参数参考上面多相机标定得到的camchain-..multicameras_calibration_2020-10-29-20-19-06.yaml文件，没有的参数可以删除，最终结果示例如下：
  
• 步骤二：编写imu.yaml，格式参考https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的imu.yaml，具体参数使用之前imu标定得到的d435i_imu_param.yaml文件，示例如下：
  
• 步骤三：准备好之前的april_6x6_50x50cm_A4.yaml，示例如下：
  
• 步骤四：复制realsense-ros包中rs_camera.launch，重命名为rs_imu_stereo.launch，更改内容为：

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

• 步骤五：启动realsense：

roslaunch realsense2_camera rs_imu_stereo.launch

• 步骤六：关闭IR结构光，参考上面：

rosrun rqt_reconfigure rqt_reconfigure

打开后将camera->stereo_module中的emitter_enabled取消勾选，展示如下：

• 步骤七：打开rviz，add imu topic和infra1 topic以及infra2 topic，同时调整realsense位置，要确保双目图像数据一直包含标定板全部内容。
  
• 步骤八：调整imu和双目topic的发布频率，以新的topic名发布，其中双目图像的发布频率改为20Hz，imu发布频率改为200Hz：

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名称可以查看实际的频率，可以发现实际频率和设置的频率并不一定相同，但可以先这样，知道如何调整的还望告知。

• 步骤九：开始录制数据包，录制过程参考[https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop]，同样注意双目图像在整个过程要包含整个标定板，同时运动不能太快，这样会造成图像过于模糊，在前后左右上下方向来回移动，录制大概90秒，前后15秒等下可以不使用。录制命令为：

rosbag record -o imu_stereo /infra_left /infra_right /imu

• 步骤十：开始进行标定，标定命令为：

kalibr_calibrate_imu_camera --bag [filename.bag] --cam [camchain.yaml] --imu [imu.yaml] --target [target.yaml] --bag-from-to 15 75 --show-extraction

相应参数需要相应更改，target.yaml对应april_6x6_50x50cm_A4.yaml文件。如：

kalibr_calibrate_imu_camera --bag ../imu_stereo_2020-10-30-11-24-41.bag --cam ../Aprilgrid/camchain.yaml --imu ../Aprilgrid/imu.yaml --target ../Aprilgrid/april_6x6_50x50cm_A4.yaml --show-extraction

（我在实验过程中没有加--bag-from-to 15 75，所以用的是所有的数据，时间比较长。）
标定过程可能出现以下错误：
提示：Optimization failed in kalibr_calibration_imu_camera，解决方法参考：https://github.com/ethz-asl/kalibr/issues/41
最终标定完成得到的结果为以下yaml、txt和pdf文件：

4. rgb相机标定

（以下为使用kalibr标定单目相机的过程，参考他人的博客，可是我最终没有成功，所以建议使用ROS包进行单目的标定。以下步骤仅供参考，慎重操作！）

• 步骤一：启动realsense：

source devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

• 步骤二：打开rviz，add color topic，同时调整realsense位置，要确保rgb图像数据一直包含标定板全部内容：

• 步骤三：调整rgb相机topic的发布频率，以新的topic名发布：

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

• 步骤四：开始录制数据包，录制过程参考[https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop]，同样注意rgb图像在整个过程要包含整个标定板，同时运动不能太快，这样会造成图像过于模糊，在前后左右上下方向来回移动，录制大概90秒。录制命令为：

rosbag record -o rgb /color

• 步骤五：开始进行标定，标定命令为：

kalibr_calibrate_imu_camera --bag [filename.bag] --cam [camchain.yaml] --imu [imu.yaml] --target [target.yaml] --bag-from-to 15 75 --show-extraction 
例如：
kalibr_calibrate_cameras --target ../Aprilgrid/april_6x6_50x50cm_A4.yaml --bag ../rgb_2020-10-30-18-02-11.bag --models pinhole-equi --topics /color --show-extraction --approx-sync 0.04

其中：

1. –target ../Aprilgrid/april_6x6_50x50cm_A4.yaml是标定板的配置文件，注意如果选择棋格盘，注意targetCols和targetRows表示的是内侧角点的数量，不是格子数量；
2. –bag ../rgb_2020-10-30-18-02-11.bag是录制的数据包；
3. --models pinhole-equi表示rgb摄像头的相机模型和畸变模型（解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models，根据需要选取）；
4. --topics /color表示拍摄的rgb相机数据话题；
5. –bag-from-to 20 80表示处理bag中20-80秒的数据；
6. –show-extraction表示显示检测特征点的过程，这些参数可以相应的调整。
7. --approx-sync 0.04 是因为标定过程中出现以下问题：
   
   参考博客解释说 加上这句话可以解决，但是我试了很多次都没有解决。

参考：

1. https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2_allsobaiduend~default-4-105646064.nonecase&utm_term=d435%20realsense%20%E5%8F%8C%E7%9B%AE&spm=1000.2123.3001.4430
2. https://blog.csdn.net/weixin_40628128/article/details/95945945
3. https://blog.csdn.net/sinat_36502563/article/details/103053292