视觉SLAM | RealsenseD435i相机标定

在运行VINS-MONO、VINS-Fusion等SLAM方案的时候,需要很准确的相机参数,否则很容易漂移。本文是RealsenseD435i相机标定过程的记录,标定主要有三个步骤

  • IMU标定
  • 相机标定
  • IMU与相机联合标定

IMU标定

这里使用imu_utils标定imu,在安装imu_utils之前,要先安装code_utils,注意要安装code_utils后再下载imu_utils

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

编译时出现错误:

code_utils/backward.hpp:216:30: fatal error: elfutils/libdw.h: No such file or directory

执行apt-get install libdw-dev 安装对应的库即可解决

接下来安装imu_utils

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

安装完成后开始标定,我没有采用录制imu话题再回放bag文件的方式,而是直接现场发布imu的话题,同时运行标定的节点。执行下面的命令可以发布带有imu的话题

roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"

编写一个标定的launch文件,命名为my_calibration_imu_d435i.launch,执行该launch文件,将RealsenseD435i静止摆放两个小时

<launch>
	<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
            #imu topic的名字
           
        
           #标定的时长
        
    node>	
launch>

完成后就会生成d435i_imu_param.yaml文件

相机标定

相机的参数分为内参和外参,内参可以通过读取/camera/color/camera_info话题获得,但是外参就需要借助别的工具,这里我使用kalibr来获取相机的外参。

安装Kalibr

首先安装依赖项

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-modules 
sudo apt-get install python-software-properties 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 pip install python-igraph --upgrade

然而在执行sudo pip install python-igraph时报错

Could not compile the C core of igraph.

本来想忽略这个问题,但是后续步骤会继续报错,没办法必须解决这个问题才行。网上的解决方法是先执行

sudo apt-get install -y libigraph0-dev

再继续安装python-igraph

但是我执行完后还是报一样的错,继续搜索,先执行

sudo apt install build-essential python-dev libxml2 libxml2-dev zlib1g-dev bison flex

再执行

 pip install -i https://pypi.tuna.tsinghua.edu.cn/simple python-igraph

中间的-i https://pypi.tuna.tsinghua.edu.cn/simple作用是使用国内的清华源,这样下载会快一点,然后莫名其妙就好了,应该是少安装了依赖项,结果如下:

Successfully built python-igraph
Installing collected packages: texttable, python-igraph
Successfully installed python-igraph-0.8.2 texttable-1.6.2

之后就开始下载安装kalibr,先另外创建一个工作空间,防止损坏原来工作空间的程序

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

然后下载并使用catkin build编译kalibr

cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j2

在使用catkin build之前,建议先将suitesparse下载下来,并修改CMakelist.txt,因为这个东西可能会由于一些原因下载时间过长,导致编译失败,详见:Camera-IMU标定工具Kalibr的编译

最后执行source ~/kalibr_workspace/devel/setup.bash

标定过程

  1. 准备标定材料

首先可以从这里下载标定材料,但是普通网友(比如我)似乎下载不了,不过还好它提供了本地生成标定材料的方法,使用kalibr_create_target_pdf --h命令,参照这里

但是在使用时报错ImportError: No module named pyx,解决方法:使用udo apt-get install python-pyx安装pyx即可

接下来生成标定材料,我选择生成官方推荐的Aprilgrid

kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]

其中:NUM_COLS、NUM_ROWS、TAG_WIDTH_M、TAG_SPACING_PERCENT是要设置的参数,含义见参考链接,我输入的指令为:

kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.02 --tspace 0.3

然后当前路径下就会出现target.pdf tmpENRhM0.tex tmpGUqWqP.tex这几个文件

标定时还需要编写target.yaml文件,如下,用来描述标定板的参数

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.02           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize
                         #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
  1. 录制相机的标定数据

使用如下命令发布图像话题,其中throttle用于将图像帧率降低到4FPS,再将新的图像话题改名为/color进行发布

roslaunch realsense2_camera rs_camera.launch
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color

接下来录制rosbag,-O之后可以设置.bag文件的名字,使用Ctrl+C结束录制

mkdir bagfiles
cd bagfiles
rosbag record -O camd435i /color
  1. 标定单个相机

直接进行标定:

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

然后可能报错

Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance

百度找到一个解决方法,直接在末尾加上--approx-sync 0.04

但还是报错,继续百度

最终解决方法:https://github.com/ethz-asl/kalibr/issues/332

src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras中如下的代码注释掉

if not graph.isGraphConnected():
    obsdb.printTable()
    print "Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance."
    graph.plotGraph()
    sys.exit(-1)

虽然感觉有点不妥。。但没报错了,标定完成后当前路径下会出现一个.yaml文件

IMU与相机联合标定

  1. 录制标定数据

仍然使用之前的标定板,运行下面的命令发布图像和IMU话题

roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"

将图像频率设置为20Hz,IMU频率设置为200Hz:

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

录制rosbag

rosbag record -O dynamic /color /imu
  1. 开始标定

新建一个camchain.yaml文件,内容如下,将intrinsicsdistortion_coeffs的数值改成之前标定的结果

cam0:
  camera_model: pinhole
  intrinsics: [637.1779587247433, 633.7804879350356, 317.29745787695197, 222.79306023564143]
  distortion_model: equidistant
  distortion_coeffs: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /color
  resolution: [640, 480]

再新建一个imu.yaml文件,内容如下,将下面的四个参数的值设置为imu标定的结果。

rostopic: /imu
update_rate: 200.0 #Hz
 
accelerometer_noise_density: 0.030017 #continous
accelerometer_random_walk: 0.000876 
gyroscope_noise_density: 0.00294 #continous
gyroscope_random_walk: 3.14268e-05

进行标定

kalibr_calibrate_imu_camera --target ~/bagfiles/target.yaml --cam ~/bagfiles/camchain.yaml --imu ~/bagfiles/imu.yaml --bag ~/bagfiles/dynamic.bag --show-extraction

标定结果如下:

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0):        mean 0.632165457077, median 0.51915755434, std: 0.418834270707
Accelerometer error (imu0):    mean 0.315519030434, median 0.26144132228, std: 0.220137274831

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0) [rad/s]:     mean 0.026284098714, median 0.0215854698381, std: 0.017414240517
Accelerometer error (imu0) [m/s^2]: mean 0.133939243527, median 0.110983013877, std: 0.0934492604853

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.99914747 -0.03946207 -0.01212755  0.04307462]
 [ 0.03965755  0.99907997  0.01632454 -0.01789784]
 [ 0.0114722  -0.01679157  0.99979319 -0.01402533]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.99914747  0.03965755  0.0114722  -0.04216722]
 [-0.03946207  0.99907997 -0.01679157  0.01934568]
 [-0.01212755  0.01632454  0.99979319  0.01483699]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0302255306475


Gravity vector in target coords: [m/s^2]
[-0.48824837 -9.78365493 -0.45840216]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [637.1779587247433, 633.7804879350356]
  Principal point: [317.29745787695197, 222.79306023564143]
  Distortion model: equidistant
  Distortion coefficients: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.02 [m]
    Spacing 0.006 [m]



IMU configuration
=================

IMU0:
----------------------------
  Model: calibrated
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.030017 
    Noise density (discrete): 0.424504485018 
    Random walk: 0.000876
  Gyroscope:
    Noise density: 0.00294
    Noise density (discrete): 0.0415778787338 
    Random walk: 3.14268e-05
  T_i_b
    [[ 1.  0.  0.  0.]
     [ 0.  1.  0.  0.]
     [ 0.  0.  1.  0.]
     [ 0.  0.  0.  1.]]
  time offset with respect to IMU0: 0.0 [s]

参考

用imu_utils标定IMU

Ubuntu18.04+ROS-melodic下的kalibr安装编译

Kalibr标定Intel D435i相机

你可能感兴趣的:(移动机器人)