【手眼标定】使用kalibr对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]