使用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.bash
ros2 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: pinhole
distortion_coeffs: [-0.17969016421219008, 0.05576666461251156, 0.0016209134649260832, 0.0009481489974055125]
distortion_model: radtan
intrinsics: [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: pinhole
distortion_coeffs: [-0.18534759506889162, 0.07196655593564935, 0.0015543265305883987, 0.003219336127464086]
distortion_model: radtan
intrinsics: [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.bash
ros2 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.bash
rosbag 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-dev
sudo 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-dev
sudo apt install libgtk-3-dev
pip install numpy==1.21
pip 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-07
Residuals
----------------------------
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-09
Transformation (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.0552938932701
Transformation (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.102528581781
Baselines:
----------
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: pinhole
Focal length: [557.1217108511009, 556.8106438635324]
Principal point: [642.3927048356297, 379.35829636447596]
Distortion model: radtan
Distortion coefficients: [-0.17969016421219008, 0.05576666461251156, 0.0016209134649260832, 0.0009481489974055125]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.021 [m]
Spacing 0.006000204 [m]
cam1
-----
Camera model: pinhole
Focal length: [559.8892318701168, 560.5548115446029]
Principal point: [660.5508624473196, 408.46268966459587]
Distortion model: radtan
Distortion coefficients: [-0.18534759506889162, 0.07196655593564935, 0.0015543265305883987, 0.003219336127464086]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.021 [m]
Spacing 0.006000204 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 10.0
Accelerometer:
Noise density: 0.00394276474817
Noise density (discrete): 0.0124681168824
Random walk: 3.18793819883e-05
Gyroscope:
Noise density: 0.016311229883
Noise density (discrete): 0.0515806378689
Random walk: 0.000282102576474
T_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]
