使用kalibr对imu和双目摄像头进行联合标定
- 前言
- 一、IMU标定
- 二、双目摄像头标定
- 三、手眼标定(imu和双目摄像头的联合标定)
前言
由于本文的imu、双目摄像头都是在ros2环境下开发,数据传输自然也是在ros2中。
但想要使用kalibr进行标定,就需要安装ros1,在ros1的环境下录制bag包提供给kalibr进行标定。所以本文在板端同时安装ros1和ros2,使用ros1_bridge允许ROS1节点和ROS2节点在同一个系统中运行,并相互通信,使得ros1环境下可以直接录制ros2发布的话题数据!
一、IMU标定
首先需要自己的系统中能够发布imu数据到某个话题,这里因人而异,我使用的是IMU40609D六轴传感器,对于大部分人应该都是买的成品,或者实验室有现成的,只需要运行某个ros指令能将imu的原始数据发布出来即可。本文使用Allan_Variance_ROS功能包来进行标定,官方建议imu的发布频率是200hz,自己调好即可。
预备:
对于使用ros2开发的小伙伴来说,就需要安装ros1和ros1_bridge。使用ros1开发的小伙伴可以参考其它博主教程,基本上联合标定他们都是在ros1中直接开发的。
①安装ros1,直接使用鱼香ros的命令:
wget http://fishros.com/install -O fishros && . fishros
②安装ros1_bridge,在ros2的环境中执行(source /opt/ros/foxy/setup.bash ):
sudo apt install ros-foxy-ros1-bridge
imu数据录制
需要开启四个终端,ros1,ros2环境都用到
①ros1环境下开启roscore:
source /opt/ros/noetic/setup.bash
roscore
②ros1、ros2环境下运行bridge:
source /opt/ros/foxy/setup.bash
source /opt/ros/noetic/setup.bashros2 run ros1_bridge dynamic_bridge --bridge-all-topics
③ros2环境下运行imu数据发布节点:
source /opt/ros/foxy/setup.bash
source install/setup.bash
ros2 run imu_test_node imu_test_node #换成自己的发布数据指令,记住发布数据的话题名称,一会儿录制,发布频率改为200hz
④ros1环境下运行rosbag进行录制:
source /opt/ros/noetic/setup.bash
rosbag record -o imu_rawdata.bag /data #/data是我imu数据话题,可以先使用ros2 topic hz /data查看发布的频率是否达到200hz
录制三个小时以上即可,在同级目录会生成bag包,后面标定会用到。
imu标定
在ros1环境下进行。开启新终端,安装、编译Allan_Variance_ROS功能包:
source /opt/ros/noetic/setup.bash
mkdir -p ~/imu_calibration_ws/src
cd ~/imu_calibration_ws/src
git clone https://github.com/ori-drs/allan_variance_ros.git
cd ..
catkin_make
source devel/setup.bash
只需要开启两终端:
①ros1环境下开启roscore:
source /opt/ros/noetic/setup.bash
roscore
②ros1环境下运行Allan_Variance_ROS进行标定:
1、对前面保存的bag按时间戳重新组织 ROS 消息:
rosrun allan_variance_ros cookbag.py --input ../Documents/imu_rawdata_2024-06-22-08-47-14.bag --output imu_cookbag.bag
--input参数:原始数据包的路径--output参数:时间戳排序后的数据包名称
2、查看bag录制时间:
rosbag play --pause ./imu_cookbag.bag
Duration后的数字向下取整即可,后面需要填入yaml文件中
3、在工作空间目录新建config文件夹,文``件夹里新建config.yaml文件,填入:
imu_topic: '/data' #imu话题
imu_rate: 200 #imu发布频率
measure_rate: 200 # Rate to which imu data is subsampled
sequence_time: 24952 #bag包时间
4、运行Allan_Variance_ROS方差计算工具:
rosrun allan_variance_ros allan_variance ./ config/config.yaml
/指的是bag包存放的路径,config/config.yaml是前面新建的yaml的路径。运行结束之后生成的.csv文件保存在当前目录下
5、运行Allan_Variance_ROS可视化绘图并获取参数:
rosrun allan_variance_ros analysis.py --data allan_variance.csv
会在同级目录下生成校准文件imu.yaml和加速度、角速度曲线图。至此imu标定结束!
#imu.yaml 下面的话题和频率需要自己手动改成自己对应的,之后kalibr进行手眼标定时会用到。
#Accelerometer
accelerometer_noise_density: 0.0064937090556434415
accelerometer_random_walk: 4.0599218203799446e-05 #Gyroscope
gyroscope_noise_density: 0.005704862739811861
gyroscope_random_walk: 0.00039076770788208285 rostopic: '/data #Make sure this is correct
update_rate: 200.0 #Make sure this is correct
二、双目摄像头标定
双目摄像头标定单独写过一篇博客,直接看这篇博客即可:使用kalibr进行双目摄像头标定
三、手眼标定(imu和双目摄像头的联合标定)
板端
板端需要准备四个文件:imu标定参数、双目相机标定参数、标定板yaml、录制imu,双目_imu的bag。
前三个文件是已经有的,分别列出来:
①imu.yaml
#Accelerometer
accelerometer_noise_density: 0.0064937090556434415
accelerometer_random_walk: 4.0599218203799446e-05 #Gyroscope
gyroscope_noise_density: 0.005704862739811861
gyroscope_random_walk: 0.00039076770788208285 rostopic: '/data #Make sure this is correct
update_rate: 200.0 #Make sure this is correct
②stereo_cam.yaml(使用opencv标定得到的畸变系数和使用kalibr标定得到的畸变系数不一样,多了一个k3,不知道能不能直接用opnecv得到的参数进行标定)
cam0:cam_overlaps: [1]camera_model: pinholedistortion_coeffs: [-0.17969016421219008, 0.05576666461251156, 0.0016209134649260832, 0.0009481489974055125]distortion_model: radtanintrinsics: [557.1217108511009, 556.8106438635324, 642.3927048356297, 379.35829636447596]resolution: [1280, 800]rostopic: /image_raw_right
cam1:T_cn_cnm1:- [0.9998667767511064, 0.0008779927232403428, -0.016299014636997042, 0.06913922407160743]- [-0.0009892592697515817, 0.9999762557402679, -0.0068197743201160455, 0.00010461888423681866]- [0.01629263991673287, 0.006834989718941845, 0.999843904217099, 0.0018747285708525514]- [0.0, 0.0, 0.0, 1.0]cam_overlaps: [0]camera_model: pinholedistortion_coeffs: [-0.18534759506889162, 0.07196655593564935, 0.0015543265305883987, 0.003219336127464086]distortion_model: radtanintrinsics: [559.8892318701168, 560.5548115446029, 660.5508624473196, 408.46268966459587]resolution: [1280, 800]rostopic: /image_raw_left
③april_6_6.yaml
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.021 #size of apriltag, edge to edge [m]
tagSpacing: 0.285724 #ratio of space between tags to tagSize
codeOffset: 0 #code offset for the first tag in the aprilboard
现在只需要录制双目摄像头和imu的bag即可:
1、ros1环境下开启roscore:
source /opt/ros/noetic/setup.bash
roscore
2、ros1、ros2环境下运行bridge:
source /opt/ros/foxy/setup.bash
source /opt/ros/noetic/setup.bashros2 run ros1_bridge dynamic_bridge --bridge-all-topics
3、ros2环境下运行imu数据发布节点:
source /opt/ros/foxy/setup.bash
source install/setup.bash
ros2 run imu_test_node imu_test_node #换成自己的发布数据指令,记住发布数据的话题名称,一会儿录制,发布频率改为200hz
4、ros2环境下运行双目摄像头发布节点:
source /opt/ros/foxy/setup.bash
source install/setup.bash ros2 launch hobot_t1_cam mipi_cam.launch.py mipi_out_format:=bgr8 mipi_io_method:=ros mipi_framerate:=4 #换成自己的发布数据指令,记住发布数据的话题名称,一会儿录制,相机发布频率改为4hz
5、ros1环境下运行rosbag进行录制:
source /opt/ros/noetic/setup.bashrosbag record /data /image_raw_right /image_raw_left -O imu_stereo.bag #/data是imu数据话题, /image_raw_right /image_raw_left分别是右左相机的话题
录制的要点是绕偏航、俯仰、翻滚3个轴旋转,上下左右前后平移,再加一些随机动作,官方给出了示例视频,有人搬运到了b站:
https://www.bilibili.com/video/av795841344/?vd_source=2a10d30b8351190ea06d85c5d0bfcb2a
录制完成之后就可以标定了。
虚拟机ubuntu端
由于手眼标定是很耗时的,并且板端不知道要跑多久,所以在虚拟机中进行。虚拟机中配置也参考双目摄像头标定那篇文章,按照里面装好kalibr,现在再安装一些手眼标定会用到的依赖:
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-devsudo apt-get install libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-devsudo apt install libgtk-3-devpip install numpy==1.21pip install wxPython
安装结束之后虚拟机端就配置好了。
进入kalibr工作空间,在里面创建config文件夹,随便哪都行,后面运行时会指定路径,将上面的四个文件拷贝到config文件夹中,终端首先开一个roscore,另一个在工作空间中执行:
rosrun kalibr kalibr_calibrate_imu_camera --target src/kalibr/april_6_6.yaml --cam src/kalibr/cam_chain.yaml --imu src/kalibr/imu.yaml --bag src/kalibr/stereocam_imu.bag --timeoffset-padding 0.1
将上述的yaml文件和bag文件路径换成自己的即可。由于第一次报了优化相关错误,所以在命令最后加上–timeoffset-padding 0.1,后面这个数字越大计算时间越久,0.1大概一个半小时。由于电脑和bag的情况不一样,0.1并不适用,如果继续报优化相关错误就增大,如果报内存溢出就减小。
标定结束后会生成xxx-camchain-imucam.yaml,xxx-report-imu.yaml,xxx-report-imucam.pdf,xxx-results-imucam.txt 四个文件。其中.txt文件中的部分结果,最重要的是相机与imu的变换矩阵(相机到IMU(T_ci),IMU到相机都有(T_ic))
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.392266281765, median 0.369929098336, std: 0.19958606249
Reprojection error (cam1): mean 0.375133761186, median 0.359180980381, std: 0.184717708985
Gyroscope error (imu0): mean 3.8491659049e-10, median 7.60223123076e-13, std: 6.26981798021e-09
Accelerometer error (imu0): mean 3.87002731774e-09, median 2.88022512412e-11, std: 1.04950189834e-07Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.392266281765, median 0.369929098336, std: 0.19958606249
Reprojection error (cam1) [px]: mean 0.375133761186, median 0.359180980381, std: 0.184717708985
Gyroscope error (imu0) [rad/s]: mean 1.98542432638e-11, median 3.9212793611e-14, std: 3.23401210741e-10
Accelerometer error (imu0) [m/s^2]: mean 4.82519529359e-11, median 3.59109834953e-13, std: 1.30853123368e-09Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[-0.99979884 -0.01950712 0.00466432 -0.00049002][ 0.01951878 -0.99980645 0.00246575 -0.00055285][ 0.00461532 0.0025563 0.99998608 0.00016617][ 0. 0. 0. 1. ]]T_ic: (cam0 to imu0):
[[-0.99979884 0.01951878 0.00461532 -0.0004799 ][-0.01950712 -0.99980645 0.0025563 -0.00056272][ 0.00466432 0.00246575 0.99998608 -0.00016252][ 0. 0. 0. 1. ]]timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0552938932701Transformation (cam1):
-----------------------
T_ci: (imu0 to cam1):
[[-0.99972373 -0.02042401 -0.01163292 0.06864607][ 0.0204759 -0.99978085 -0.0043586 -0.00044886][-0.01154135 -0.00459559 0.99992284 0.00202911][ 0. 0. 0. 1. ]]T_ic: (cam1 to imu0):
[[-0.99972373 0.0204759 -0.01154135 0.06865972][-0.02042401 -0.99978085 -0.00459559 0.00096259][-0.01163292 -0.0043586 0.99992284 -0.00123236][ 0. 0. 0. 1. ]]timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.102528581781Baselines:
----------
Baseline (cam0 to cam1):
[[ 0.99986678 0.00087799 -0.01629901 0.06913922][-0.00098926 0.99997626 -0.00681977 0.00010462][ 0.01629264 0.00683499 0.9998439 0.00187473][ 0. 0. 0. 1. ]]
baseline norm: 0.0691647154086 [m]Gravity vector in target coords: [m/s^2]
[ 0.40446197 -9.78898103 0.42506912]Calibration configuration
=========================cam0
-----Camera model: pinholeFocal length: [557.1217108511009, 556.8106438635324]Principal point: [642.3927048356297, 379.35829636447596]Distortion model: radtanDistortion coefficients: [-0.17969016421219008, 0.05576666461251156, 0.0016209134649260832, 0.0009481489974055125]Type: aprilgridTags: Rows: 6Cols: 6Size: 0.021 [m]Spacing 0.006000204 [m]cam1
-----Camera model: pinholeFocal length: [559.8892318701168, 560.5548115446029]Principal point: [660.5508624473196, 408.46268966459587]Distortion model: radtanDistortion coefficients: [-0.18534759506889162, 0.07196655593564935, 0.0015543265305883987, 0.003219336127464086]Type: aprilgridTags: Rows: 6Cols: 6Size: 0.021 [m]Spacing 0.006000204 [m]IMU configuration
=================IMU0:----------------------------Model: calibratedUpdate rate: 10.0Accelerometer:Noise density: 0.00394276474817 Noise density (discrete): 0.0124681168824 Random walk: 3.18793819883e-05Gyroscope:Noise density: 0.016311229883Noise density (discrete): 0.0515806378689 Random walk: 0.000282102576474T_ib (imu0 to imu0)[[ 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]