相机+IMU标定 Realsense D435i

 

使用的标定工具总共包含两个，一个是苏黎士联邦理工的Kalibr工具，可以标定相机内参及相机-IMU外参，另外一个是港科大的imu_utils,可以标定IMU内参，我们先标定IMU内参，再标定相机内参，最后联合标定，得到相机-IMU的外参。

一、IMU标定

1.修改rs_camera.launch部分参数

使得IMU数据同步：

找到realsense-ros工作环境文件夹中的rs_camera.launch
位置：在 /realsense的catkin工作环境/src/realsense-ros/realsense2_camera/launch/
把rs_camera.launch中的：

中的 “copy” 修改成"linear_interpolation" （注：作用是将陀螺仪与加速度计进行融合生成topic: “imu”)；

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

roscore 
roslaunch realsense2_camera rs_camera.launch
rostopic list

roscore （注：如果是roslaunch不需要特意启动roscore，如果是rosrun必须要启动roslaunch，roslaunch是启动多个节点的配置文件，rosrun启动的是一个的节点）

在第三个terminal下查看是否有/camera/imu，说明ok了。

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

  （1）安装ceres（本博主已经安装ceres）

 （2）创建ros工作空间：mkdir -p可以创建多级目录 mkdir只创建单个目录

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/
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

文件格式为：

max_time_min代表的是标定时间，单位分钟，为至少录制的时间填写30分钟。

4.录制IMU数据包

插上相机：

roslaunch realsense2_camera rs_camera.launch

让相机静止，不要动它

cd ~/imu_catkin_ws      
rosbag record -O imu_calibration /camera/imu 

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

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

（1）打开一个terminal：

source ~/imu_catkin_ws/devel/setup.sh 
roslaunch 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）的termial，有一串东西产生，而且该terminal可以输入命令，说明已经标定好了：

cd ~/imu_catkin_ws/src/imu_utils/data 

里面的文件：

d435i_imu_calibration_imu_param.yaml为想要的IMU标定结果，其中用到其中四个参数：

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

wiki中描述的是ubuntu14的环境，参照下面的博客：

https://xsin.gitee.io/2019/03/05/ubuntu16-04-kinetic%E6%BA%90%E7%A0%81%E5%AE%89%E8%A3%85kalibr%E6%A0%87%E5%AE%9A%E5%B7%A5%E5%85%B7/

安装环境：

1.安装ros-kinetic-desktop-full
2.安装kalibr源码编译所需依赖项：

sudo apt-get install python-setuptools

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev

sudo apt-get install libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-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

创建工作空间：

mkdir -p ~/kalibr_ws/src
cd ~/kalibr_ws
source /opt/ros/kinetic/setup.bash
catkin init
catkin config --extend /opt/ros/kinetic
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

开始编译：

cd ~/kalibr_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_ws
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source　~/kalibr_ws/devel/setup.sh

kalibr工具生成标定板,（博主需要生成棋盘格）：

kalibr_create_target_pdf --type 'apriltag' --nx 6 --ny 6 --tsize 0.08 --tspace 0.3

2.下载&打印标定板

https://github.com/ethz-asl/kalibr/wiki/downloads

https://github.com/ethz-asl/kalibr/wiki/calibration-targets（三个参数chekboard, circlegrid, aprilgrid说明，博主使用的是checkerboard）

target_type: 'checkerboard'  #gridtype
tagCols: 11                  #number of internal chessboard corners
tagRows: 8                   #number of internal chessboard corners
tagSize: 0.02                #size of one chessboard square [m]
tagSpacing: 0.02             #size of one chessboard square [m]

  3.确定适合距离

roslaunch realsense2_camera rs_camera.launch
rviz

左侧Fixed Frame选择camera_link

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

  4.修改相机帧数

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

修改相机帧数到4hz，这里采用了新的话题去发布：/color  第5步录制写到/color话题。

  5.录制ROS数据包

rosbag record -O camd435i /color

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

  6.使用Kalibr进行标定

在刚刚建立的ROS中的Kalibr的工作空间，执行source devel/setup.sh，执行：

kalibr_calibrate_cameras --target checkerboard.yaml --bag camd435i.bag --models pinhole-radtan --topics /color --show-extraction

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是畸变参数

三、相机IMU联合标定

官方教程：https://github.com/ethz-asl/kalibr/wiki/camera-imu-calibration

标定完camera，IMU，得到相应的yaml文件（即分别为相机和IMU的参数）

  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.录制数据包

先把目录移动到刚刚建立的kalibr工作环境目录下。
然后执行source devel/setup.sh

rosbag record -O dynamic /color /imu

  3.重写yaml文件

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

（1）相机标定的yaml文件

同样是用kalibr标定的，camchain-…camd435i.yaml，所以直接使用相机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: 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)

其中以下IMU标定参数已知，将结果对应地填到以上imu.yaml中去： 

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

  4.使用Kalibr进行标定

在Kalibr工作目录下执行 source devel/setup.sh

执行指令：

kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camd435i.yaml --imu imu.yaml --bag dynamic.bag --show-extraction

可以录制2分钟，生成文件名字为：

camchain-imucam-dynamic.yaml

其中T_cam_imu，就是我们需要的外参。

 

 

参考：

https://blog.csdn.net/Hanghang_/article/details/103546033

https://xsin.gitee.io/2019/03/05/ubuntu16-04-kinetic%E6%BA%90%E7%A0%81%E5%AE%89%E8%A3%85kalibr%E6%A0%87%E5%AE%9A%E5%B7%A5%E5%85%B7/