T265相机双目鱼眼+imu联合标定（全记录）

最近工作用到t265，记录一遍标定过程

1.安装驱动

首先安装realsense驱动，因为笔者之前使用过d435i，装的librealsense版本为2.55.1，直接使用t265会出现找不到设备的问题，经查阅发现是因为realsense在2.53.1后就不再支持t265了。

包括使用launch文件打开相机也不可以。

需要对其降级，于是到librealsense/build中执行sudo make uninstall删除。

新下载2.53.1版本：realsense各个版本

下载后，准备安装，这过程不要连接相机！

先安装依赖：

sudo apt-get install libudev-dev pkg-config libgtk-3-dev
sudo apt-get install libusb-1.0-0-dev pkg-config
sudo apt-get install libglfw3-dev
sudo apt-get install libssl-dev

然后进入到下载好并解压的文件夹内执行以下命令：

sudo cp config/99-realsense-libusb.rules /etc/udev/rules.d/
sudo udevadm control --reload-rules && udevadm trigger 
mkdir build
cd build
cmake …
make
sudo make install

完事就可以输入：

realsense-viewer

此时应该显示t265双目鱼眼相机的画面了

好，这一步成功

2.安装Realsense ROS

创建ros工作空间：

mkdir -p ~/realsense_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

然后下载文件：

git clone https://github.com/IntelRealSense/realsense-ros.git
git clone https://github.com/pal-robotics/ddynamic_reconfigure.git

编译并运行：

cd ..
catkin_make 
source devel/setup.bash
roslaunch realsense2_camera demo_t265.launch

就可以在rviz中添加话题查看左右目图像了。

3.安装imu标定工具：

code_utils：

mkdir -p ~/code_utils_ws/src
cd code_utils_ws/
catkin_make

下载包并编译：

sudo apt-get install libdw-dev
cd ~/code_utils_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

会报错：显示没有backward.hpp

只需要在include_directories中加入路径就好，如图：

再编译就好了。

然后下载imu_utils功能包，并编译：

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

这次很顺利。

然后需要在静置条件下录制imu包，我录为2h，在录包的时候一定要执行roslaunch rs_t265.launch，使相机工作。

 执行开始录制包：

rosbag record -O imu2h /camera/imu --duration=2h

然后标定过程需要在/home/shikai/code_utils_ws/src/imu_utils/launch这个路径下新建一个launch文件

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <param name="imu_name" type="string" value= "imu"/>
        <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>

主要修改 imu_topic为自己的imu话题和max_time_min时长，单位是分钟。

roslaunch imu_test.launch
rosbag play -r 200 imu2h.bag

这一步分开执行，有的博主没写清楚，执行第一步之后，程序会进入等待话题的状态，这时再执行第二步快进过一遍自己的rosbag，当bag包加速回放完毕后，执行launch的窗口仍然会显示wait for imu data.，等待一段时间计算，计算完毕后会显示计算结果。显示done之后，在/home/shikai/code_utils_ws/src/imu_utils/data这个文件夹下会出现一系列的data文件，打开yaml文件，会看到计算出来的噪声和随机游走的系数值。

到此时对imu标定完毕

4.安装kalibr：

这个网上有好多教程，不赘述，直接说录包命令

rosbag record -O test1 /camera/fisheye1/image_raw /camera/fisheye2/image_raw

对着标定板录包后，该实用kalibr进行标定了

rosrun kalibr kalibr_calibrate_cameras --target April.yaml --bag test1.bag --models omni-radtan omni-radtan --topics /camera/fisheye1/image_raw /camera/fisheye2/image_raw

rosrun kalibr kalibr_calibrate_cameras：这是ROS的命令格式，用于运行Kalibr包中的kalibr_calibrate_cameras程序。

--target April.yaml：这个参数指定了用于相机校准的标定板（标定靶）的配置文件。

--bag test1.bag：这个参数指定了包含相机图像数据的ROS bag文件。test1.bag是一个bag文件，其中包含了用于相机校准的图像数据。

--models omni-radtan omni-radtan：这个参数指定了相机模型的类型。omni-radtan是一种用于鱼眼相机的畸变模型，它考虑了径向畸变和切向畸变，适用于鱼眼镜头产生的强烈畸变。

--topics /camera/fisheye1/image_raw /camera/fisheye2/image_raw：这个参数指定了ROS话题

然后经过漫长的等待，最终得到双目相机标定结果

根据报告来看效果还可以 。

5.联合标定

首先新建文件touch imu.yaml

之前imu标定有一个yaml文件内容是这样的：

%YAML:1.0
---
type: IMU
name: imu
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.0329310164723647e-03
      gyr_w: 2.2123382727191195e-05
   x-axis:
      gyr_n: 1.7189864293645056e-03
      gyr_w: 2.6415569244764821e-05
   y-axis:
      gyr_n: 2.7012961183810300e-03
      gyr_w: 2.2206632567010946e-05
   z-axis:
      gyr_n: 1.6785105016715576e-03
      gyr_w: 1.7747946369797813e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.8852789385037937e-02
      acc_w: 4.0146583082726104e-04
   x-axis:
      acc_n: 1.6792437581239685e-02
      acc_w: 2.8762304968836755e-04
   y-axis:
      acc_n: 1.8515839456546758e-02
      acc_w: 5.7417816070608088e-04
   z-axis:
      acc_n: 2.1250091117327372e-02
      acc_w: 3.4259628208733464e-04

我们新建imu文件使用其中内容即可：

rostopic: /camera/imu
update_rate: 10.0 #Hz
 
accelerometer_noise_density: 1.8852789385037937e-02
accelerometer_random_walk: 4.0146583082726104e-04
gyroscope_noise_density: 2.0329310164723647e-03
gyroscope_random_walk: 2.2123382727191195e-05

这样就得到imu.yaml文件，加上之前得到t265相机标定yaml文件来联合标定，执行以下命令：

rosrun kalibr kalibr_calibrate_imu_camera --target April.yaml --bag test1.bag --cams test1-camchain.yaml --imu imu.yaml

等待好久，要一直优化，终端会显示这些，一直在迭代：

Optimizing...
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Using the block_cholesky linear system solver
Using the levenberg_marquardt trust region policy
Initializing
Optimization problem initialized with 17204 design variables and 656142 error terms
The Jacobian matrix is 1346674 x 77398
[0.0]: J: 1.18472e+06
[1]: J: 101749, dJ: 1.08297e+06, deltaX: 0.203747, LM - lambda:10 mu:2
[2]: J: 98905.9, dJ: 2843.45, deltaX: 0.0720314, LM - lambda:3.33333 mu:2
[3]: J: 98553.1, dJ: 352.821, deltaX: 0.0682763, LM - lambda:1.11111 mu:2
[4]: J: 98543.7, dJ: 9.38912, deltaX: 0.150521, LM - lambda:0.37037 mu:2

最终输出结果，外参、时间戳延迟之类：

T_ci:  (imu0 to cam0): 
[[-0.99999467  0.00187288 -0.00267487  0.01599095]
 [-0.00187503 -0.99999792  0.00080112  0.02327695]
 [-0.00267337  0.00080613  0.9999961  -0.01275659]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[-0.99999467 -0.00187503 -0.00267337  0.0160004 ]
 [ 0.00187288 -0.99999792  0.00080613  0.02325724]
 [-0.00267487  0.00080112  0.9999961   0.01278067]
 [ 0.          0.          0.          1.        ]]

到这里就结束了