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-03gyr_w: 2.2123382727191195e-05x-axis:gyr_n: 1.7189864293645056e-03gyr_w: 2.6415569244764821e-05y-axis:gyr_n: 2.7012961183810300e-03gyr_w: 2.2206632567010946e-05z-axis:gyr_n: 1.6785105016715576e-03gyr_w: 1.7747946369797813e-05
Acc:unit: " m/s^2"avg-axis:acc_n: 1.8852789385037937e-02acc_w: 4.0146583082726104e-04x-axis:acc_n: 1.6792437581239685e-02acc_w: 2.8762304968836755e-04y-axis:acc_n: 1.8515839456546758e-02acc_w: 5.7417816070608088e-04z-axis:acc_n: 2.1250091117327372e-02acc_w: 3.4259628208733464e-04
我们新建imu文件使用其中内容即可:
rostopic: /camera/imu
update_rate: 200.0 #Hzaccelerometer_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. ]]
到这里就结束了