相机与IMU联合标定
1、 imu_utils标定IMU的内参，可以校准IMU的噪声密度和随机游走噪声
2、kalibr包标定相机的内外参数，相机与IMU之间的外参
这里我所使用的相机是ZED相机，IMU使用的是Xsens的IMU
1 imu_utils 标定IMU的内参
1.1 安装环境
这里使用的包是 imu_utils [1],使用这个包可以校准IMU的噪声密度和随机游走噪声
step1： 安装ceres库
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev
sudo apt-get install libgoogle-glog-dev libgtest-dev
下载编译 ceres-solver
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make
sudo make install
step2： 安装 code_utils
构建工作空间
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
catkin_make
注意,这里需要修改 code_utils 的 CMakeLists.txt 文件, CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14 并在添加 include_directories(include/code_utils)
sudo apt-get install libdw-dev
cd kalibr_workspace/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
step3： 安装 imu_utils
注意,这里需要t同样修改 imu_utils 的 CMakeLists.txt 文件, CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14
cd kalibr_workspace/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
1.2 录制IMU数据集
step4: 静止情况下采集IMU的数据，并录制为ROS包，我采集的时间为2小时20分钟。
rosbag record /imu/data -O imu_xsens.bag
step5: 标定 配置xsens.launch文件为如下内容：(指定IMU的topic)
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/imu/data"/>
<param name="imu_name" type="string" value= "xsens"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "120"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>
其中, max_time_min 表示从ros bag包中加载多长时间的数据
随后，启动 imu_utils 标定IMU
rosbag play -r 200 imu_xsens.bag
roslaunch imu_utils xsens.launch
可得到如下输出结果：
%YAML:1.0
---
type: IMU
name: xsens
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 4.9700389767894345e-03
gyr_w: 6.8522312307501954e-05
x-axis:
gyr_n: 4.9016990946736316e-03
gyr_w: 7.4721244172569240e-05
y-axis:
gyr_n: 4.9561825133283884e-03
gyr_w: 6.7372212440839068e-05
z-axis:
gyr_n: 5.0522353223662842e-03
gyr_w: 6.3473480309097542e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 5.4303615468688114e-03
acc_w: 1.4459759280127568e-04
x-axis:
acc_n: 5.7602300667994426e-03
acc_w: 1.5064216759502606e-04
y-axis:
acc_n: 5.6378382747581945e-03
acc_w: 1.4458085233539733e-04
z-axis:
acc_n: 4.8930162990487953e-03
acc_w: 1.3856975847340367e-04
2 kalibr 标定工具
2.1 安装
安装依赖项：
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
编译kalibr,这里要确保电脑联网,编译时间有点长.
cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source ~/kalibr_workspace/devel/setup.bash
2.2 校准相机的内外参
step1: 改变ZED相机发布的频率
rosrun topic_tools throttle messages /camera/left/image_raw 4.0 /stereo/left/image_raw
rosrun topic_tools throttle messages /camera/right/image_raw 4.0 /stereo/right/image_raw
录制ROS bag 包
rosbag record /stereo/left/image_raw /stereo/right/image_raw -O zed_images.bag
step2: 标定
kalibr_calibrate_cameras --target april_6x6_24x24mm.yaml --bag images.bag --bag-from-to 5 30 \
--models pinhole-radtan pinhole-radtan --topics /stereo/left/image_raw /stereo/right/image_raw
这里还有其他的工具包也可以用 ROS camera_calibration 包。标定成功以后会在目录下生成一个 yaml文件(用于后续校准相机和IMU外参) 和 一个pdf校准日志,如果校准的重投影误差在1个像素内就是比较好的结果了.
2.3 校准相机与IMU外参
step1: 把IMU和相机固定在一起录制ROS bag包, 录制的时候要注意按照官方的说法-充分激励IMU- 绕3个轴旋转和3个方向的平移，这里有个官方视频[3] 可以参考
rosbag record /imu/data_raw /stereo/left/image_raw /stereo/right/image_raw -O images_imu.bag
step2: 启动校准包开始校准
kalibr_calibrate_imu_camera --target april_6x6_55x55mm.yaml --bag ~/bagfiles/zed_xsens.bag --bag-from-to 5 50 --cam camchain_images_zed_3495.yaml --imu xsens_imu.yaml --imu-models scale-misalignment --timeoffset-padding 0.1
其中 参数：
1） –target april_6x6_24x24mm.yaml 描述标定板的信息
2） –bag images_imu.bag 指定数据包
3） –bag-from-to 5 50 设定bag包开始时间和结束时间，避开拿起和放下IMU的时间段内的数据
4）–cam camchain_images_zed_3495.yaml 相机参数文件
5）–imu xsens_imu.yaml 设定IMU的信息
6）–imu-models scale-misalignment IMU的参数模型
camchain_images_zed_3495.yaml 文件内容如下:
cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [-0.1734857772863602, 0.026545178121976657, 0.0004291887376674085,
-3.4873170616746686e-05]
distortion_model: radtan
intrinsics: [693.131838769146, 692.5498277671763, 616.3486206381017, 379.6677572220899]
resolution: [1280, 720]
rostopic: /stereo/left/image_raw
cam1:
T_cn_cnm1:
- [0.9999658061828064, 0.0005632950824424241, 0.0082504038578218, -0.11947602055948268]
- [-0.0006621128372211408, 0.9999280240823567, 0.011979493367486592, 0.0004870068672051519]
- [-0.008243062037729159, -0.011984546441186855, 0.9998942056912012, -0.0028910358303400464]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [-0.17456713089475154, 0.027410444232267236, 0.0006360696559962682,
-0.0002450168896166665]
distortion_model: radtan
intrinsics: [694.2107729740508, 693.480347118504, 617.3114354961933, 380.800130116761]
resolution: [1280, 720]
rostopic: /stereo/right/image_raw
xsens_imu.yaml 文件内容:
#Accelerometers
accelerometer_noise_density: 5.43036e-03 #Noise density (continuous-time)
accelerometer_random_walk: 1.44598e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 4.9700e-03 #Noise density (continuous-time)
gyroscope_random_walk: 6.8522e-05 #Bias random walk
rostopic: /imu/data #the IMU ROS topic
update_rate: 100.0 #Hz (for discretization of the values above)
标定完成以后同样会生成一个报表和在终端里面打印校准信息;
标定结果的重投影误差应该在零点几个像素
标定出来的位移和实际测量或估计的IMU 和相机中心位移比较一致
总结
- 相机和IMU标定流程比较简单
- 校准时候要注意限制相机的频率为4HZ,校准IMU时候采集数据的时间在2小时左右
- 利用kalibr标定的精度看起来还是OK的
参考资料
[1] https://github.com/gaowenliang/imu_utils
[2] https://blog.csdn.net/u011178262/article/details/83316968?utm_medium=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.add_param_isCf&depth_1-utm_source=distribute.pc_relevant_t0.none-task-blog-BlogCommendFromMachineLearnPai2-1.add_param_isCf
[3] https://www.bilibili.com/video/av795841344/