- 1C++从入门到进阶的系列书籍推荐（22本书）_c++书籍
- 2大数据篇--中台概念总结_数据中台 起名
- 3[晓理紫]每日论文推送(有中文摘要，源码或项目地址)--大模型相关、扩散模型、视觉导航_crossway diffusion
- 4git提交或克隆报错fatal: unable to access_unable to access git
- 5AIGC引领保险数智化变革报告
- 6专科学历，五面京东成功斩获Offer_专科能进京东吗
- 7什么是进程？C语言
- 8在IIS服务器上安装SSL证书/网站通过HTTPS协议访问的配置方法_iis ssl配置
- 9ubuntu配置pip 清华源_ubuntu pip 清华源
- 10Linux的磁盘管理与文件系统_xfs文件系统和磁盘硬件的关系
D435i相机固件升级-imu校准-单目和imu及双目和imu外参标定-VINSfusion运行双目+imu（完整版）_realsense d435i rgb相机到imu的参数
赞
踩
本机环境介绍
ubuntu20.04
ceres1.14.0
Ros环境
realsense-sdk2.0（d435i的驱动等，直到可以运行realsense-viewer，可以看到图像和深度图）
本次标定使用两个工具，一个是kalibr工具，可以标定相机内参（本实验没有使用）以及相机-IMU外参，另一个是港科大出品的imu_utils，可以标定IMU内参，我们先标定IMU内参，再用ros工具标定相机内参，最后联合标定，得到相机-IMU的外参。
一、IMU标定
1.修改rs_camera.launch中的参数
找到realsense-ros工作环境文件夹中的rs_camera.launch
位置：在 /realsense的catkin工作环境/src/realsense-ros/realsense2_camera/launch/把rs_camera.launch中的unite_imu_method和enable_sync改为如下所示：分别用于发布IMU信息和相机与IMU进行时间同步
<arg name="unite_imu_method" default="copy"/>
<arg name="enable_sync" default="true"/>
- 1
- 2
2.配置港科大IMU标定程序的环境
（1）安装ceres
不过多介绍：去官网下载源码编译就好了。
https://github.com/ceres-solver/ceres-solver
http://www.ceres-solver.org/
（2）为标定IMU创建ROS工作空间
mkdir -p ~/catkin_imu_ws/src
cd ~/catkin_imu_ws/src
catkin_init_workspace
cd ~/catkin_imu_ws
catkin_make
source ~/catkin_imu_ws/devel/setup.bash
- 1
- 2
- 3
- 4
- 5
- 6
（可以将对应的source命令和ROS工作路径加入.bashrc文件中，按道理将应该会更好，实际上创建了这两个工作空间以后我原来的ros包都不能识别都，都要手动进行相应的ros工作目录进行
source devel/setup.bash 后才能自动补全）
（3）先下载code_utils并编译
cd ~/catkin_imu_ws/src/
git clone https://github.com/gaowenliang/code_utils.git
cd ~/catkin_imu_ws/src/
catkin_make
- 1
- 2
- 3
- 4
（4）然后再下载imu_utils并编译
cd ~/catkin_imu_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/catkin_imu_ws/src/
catkin_make
- 1
- 2
- 3
- 4
注：（3）和（4）注意先后顺序，一个一个编译，中间有点小问题，很简单的那种，一个是头文件可能要加一下路径，还有一个要包含一下vector这个头文件。
3.编写参数配置文件
cd ~/catkin_imu_ws/src/imu_utils/launch
touch d435i_imu_calibration.launch
gedit d435i_imu_calibration.launch
- 1
- 2
- 3
直接复制粘贴下面的就行了
<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= "d435i_imu_calibration"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "10"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
注：max_time_min代表的是标定时间，这里的单位是分钟，意思是填10就是代表10分钟，具体录制多久自行把握，反正时间越长越好。
4.离线录制IMU数据包
打开d435i相机
roscore
roslaunch realsense2_camera rs_camera.launch
rqt_image_view //查看相机是否正常打开
rostopic echo /camera/imu //检查IMU话题时候有输出
rostopic hz /camera/imu //或者检查IMU打印频率
- 1
- 2
- 3
- 4
- 5
- 6
一切正常后不要移动相机，静置，进入想要保存bag的目录下调用终端开始录制。
rosbag record -O imu_calibration /camera/imu
- 1
至少录制你刚刚设定的时间以上(max_time_min)
比如上面设定10，这里就要录足十分钟
录制完之后就按下ctrl+c，结束录制
你会发现当前目录有一个名为 imu_calibration.bag的文件
5.使用IMU工具箱标定
打开一个终端：
source ~/catkin_imu_ws/devel/setup.bash
roslaunch imu_utils d435i_imu_calibration.launch
- 1
- 2
再调一个终端
source ~/catkin_imu_ws/devel/setup.sh
cd ~/catkin_imu_ws //如果数据包在这个文件夹下
rosbag play -r 200 imu_calibration.bag
- 1
- 2
- 3
执行完毕后即可在这个目录下/imu_catkin_ws/src/imu_utils/data 找到一个名为 d435i_imu_calibration_imu_param.yaml的文件，打开即可查看标定结果，这是15分钟的标定结果。
%YAML:1.0 --- type: IMU name: d435i_imu_calibration Gyr: unit: " rad/s" avg-axis: gyr_n: 1.8282505899879821e-03 gyr_w: 1.9435262017179183e-05 x-axis: gyr_n: 1.4728528003952847e-03 gyr_w: 1.8708803243080059e-05 y-axis: gyr_n: 2.3392615939959523e-03 gyr_w: 2.2671377122899810e-05 z-axis: gyr_n: 1.6726373755727097e-03 gyr_w: 1.6925605685557679e-05 Acc: unit: " m/s^2" avg-axis: acc_n: 1.2816582334747946e-02 acc_w: 5.0028610709826078e-04 x-axis: acc_n: 1.2169492333962395e-02 acc_w: 3.9215145394702318e-04 y-axis: acc_n: 1.5026867851384572e-02 acc_w: 7.8886749202973451e-04 z-axis: acc_n: 1.1253386818896876e-02 acc_w: 3.1983937531802474e-04
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
等下只会用到其中几个参数用于相机和IMU之间的外参标定，分别是陀螺仪和加速度计高斯白噪声和随机游走噪声的平均值，是IMU噪声模型中的两种噪声。
avg-axis:
gyr_n: 1.8282505899879821e-03
gyr_w: 1.9435262017179183e-05
avg-axis:
acc_n: 1.2816582334747946e-02
acc_w: 5.0028610709826078e-04
- 1
- 2
- 3
- 4
- 5
- 6
二、相机的标定
打开d435i相机是可以通过一个话题名称查看出厂值的，可以用于和标定结果对比
roscore
roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/color/camera_info
- 1
- 2
- 3
打印如下信息：
header: seq: 1338 stamp: secs: 1713246555 nsecs: 16008854 frame_id: "camera_color_optical_frame" height: 480 width: 640 distortion_model: "plumb_bob" D: [0.0, 0.0, 0.0, 0.0, 0.0] K: [606.0228271484375, 0.0, 325.3205871582031, 0.0, 605.7271118164062, 245.59124755859375, 0.0, 0.0, 1.0] R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] P: [606.0228271484375, 0.0, 325.3205871582031, 0.0, 0.0, 605.7271118164062, 245.59124755859375, 0.0, 0.0, 0.0, 1.0, 0.0] binning_x: 0 binning_y: 0 roi: x_offset: 0 y_offset: 0 height: 0 width: 0 do_rectify: False ---
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
但是其中的distortion_model是 “plumb_bob”，后面使用kalib工具箱时这个类型的畸变模型是不行的，所以要重新标定一下，使用ros的标定工具就行，很方便。
启动相机后：
roscore
roslaunch realsense2_camera rs_camera.launch
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.035 image:=/camera/color/image_raw
- 1
- 2
- 3
注：其中size和square的具体值看你使用的棋盘格,注意要用"x"用 "*"是不行的,最后一个d435i相机图像话题的名称。
推荐一个可以自动生成棋盘格PDF的网站：https://calib.io/pages/camera-calibration-pattern-generator
最开始的CALBRATE按钮为灰色，当采集的图像符合条件够就变成绿色，然后可以点击CALIBRATE进行标定，标定结果显示如下：
[image] width 640 height 480 [narrow_stereo] camera matrix 619.625632 0.000000 332.100914 0.000000 616.574239 246.877536 0.000000 0.000000 1.000000 distortion 0.134955 -0.247547 0.006056 0.002826 0.000000 rectification 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 projection 632.333923 0.000000 333.720494 0.000000 0.000000 628.858948 249.009700 0.000000 0.000000 0.000000 1.000000 0.000000
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
将标定结果存入camchain.yaml中用于后续相机和IMU之间的外参标定。
cam0: #640*480
camera_model: pinhole
intrinsics: [619.625632, 616.574239,332.100914, 246.877536]
distortion_model: radtan
distortion_coeffs: [0.134955, -0.247547, 0.006056, 0.002826]
rostopic: /color
resolution: [640, 480]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
其中的rostopic先这样设置，后面会解释。
三、相机和IMU联合标定
目的： 我们进行camera-IMU标定的目的是为了得到IMU和相机坐标系的相对位姿矩阵T和相对时间延时t_shift(t_imu=t_cam + t_shift)。
需要的文件：
1、checkboard.yaml:标定目标板的参数
2、IMU.yaml: 包含IMU的噪声密度、随机游走；
3、camchain.yaml: 包含相机的内参、畸变参数的文件；
4、.bag:包含有图片信息和IMU数据的ROS包
所以在进行camera-IMU 标定前，我们分别要对IMU进行标定（第1节）和 camera内参(第2节)得到相应的imu.yaml文件和camchain.yaml文件，已准备好相关文件，放到kalib文件目录下。
kalib工具箱的下载参考：https://blog.csdn.net/u011392872/article/details/106204419
（1）checkboard.yaml
使用kalib工具箱生成，正好可以验证是否安装成功。
参考文章：https://blog.csdn.net/u012210937/article/details/127600917
roscore
rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3
- 1
- 2
生成如下图片，打印出来即可：
则checkboard.yaml文件如下设置即可：
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.022 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
- 1
- 2
- 3
- 4
- 5
（2）imu.yaml文件
如下设置即可，使用前面标定出来的结果：
avg-axis:
gyr_n: 1.8282505899879821e-03
gyr_w: 1.9435262017179183e-05
avg-axis:
acc_n: 1.2816582334747946e-02
acc_w: 5.0028610709826078e-04
- 1
- 2
- 3
- 4
- 5
- 6
imu.yaml文件如下所示，其中rostopic：/imu后面会进一步介绍，先这样设置即可：
rostopic: /imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 1.28e-01 #continous
accelerometer_random_walk: 5.00e-04
gyroscope_noise_density: 1.83e-03 #continous
gyroscope_random_walk: 1.94e-05
- 1
- 2
- 3
- 4
- 5
- 6
- 7
（3）camchain.yaml文件
相机标定节已经设置完毕：
cam0: #640*480
camera_model: pinhole
intrinsics: [619.625632, 616.574239,332.100914, 246.877536]
distortion_model: radtan
distortion_coeffs: [0.134955, -0.247547, 0.006056, 0.002826]
rostopic: /color
resolution: [640, 480]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
（4）.bag文件
1.将图像频率降低为20HZ，imu频率设置为200Hz,这里可以用throttle方法，不会出错，并发布新的topic，不会修改原topic:
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/gyro/image_info 200.0 /imu
- 1
- 2
注： 将/camera/color/image_raw重新发布频率为20HZ的图像话题/color，IMU同理重新发布200HZ频率话题名为/imu
此时明白为什么camchain.yaml和imu.yaml文件中的话题名称不是相机发布的话题名称了吧。
还有一件事是相机的IMU不一定能够正常发布话题，可以用进行测试，不正常就插拔一下相机。
rostopic hz /camera/imu
- 1
2.录制ros包
rosbag record -O d435i /color /imu
- 1
注意：录制的时候相机缓慢移动，并且保证标定板不要出画，同时对IMU的三个自由度充分激励，pitch，yaw 和roll。
3.标定
roscore
rosrun kalib kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camchain.yaml --imu imu.yaml --bag d435i.bag --bag-from-to 5 45 --show-extraction
- 1
- 2
注意事项：
1.注意文件这个文件的路径;
2.-bag-from-to 5 45表示从ros包的5秒到45秒的数据进行标定;
3.–show-extraction可以实时查看标定过程，如果报错Attempt to unlock mutex that was not locked,参考博客：https://blog.csdn.net/christy9090900/article/details/129143409
因为ubuntu自带的opencv与我安装的opencv3.2.0有冲突
将kalibr中的以下三个含有 cv::startWindowThread()的cpp的cv::startWindowThread()注释掉，再重新编译kalib工具箱即可
catkin build -DCMAKE_BUILD_TYPE=Release -j4
- 1
4.标定结果展示
Calibration results =================== Normalized Residuals ---------------------------- Reprojection error (cam0): mean 0.39929005288490255, median 0.353574470294689, std: 0.24677455043783994 Gyroscope error (imu0): mean 0.19635885465367767, median 0.1638485519641198, std: 0.13718355237347882 Accelerometer error (imu0): mean 0.05405172422990145, median 0.0375137913169485, std: 0.059174792932168666 Residuals ---------------------------- Reprojection error (cam0) [px]: mean 0.39929005288490255, median 0.353574470294689, std: 0.24677455043783994 Gyroscope error (imu0) [rad/s]: mean 0.005081788402781992, median 0.004240418251840175, std: 0.003550325177510012 Accelerometer error (imu0) [m/s^2]: mean 0.09784407228873862, median 0.06790721594427214, std: 0.1071178172355744 Transformation (cam0): ----------------------- T_ci: (imu0 to cam0): [[ 0.99983681 0.01353248 -0.01196764 0.03383921] [-0.01370252 0.99980468 -0.01424262 0.02262114] [ 0.01177256 0.01440428 0.99982695 0.00121541] [ 0. 0. 0. 1. ]] T_ic: (cam0 to imu0): [[ 0.99983681 -0.01370252 0.01177256 -0.03353803] [ 0.01353248 0.99980468 0.01440428 -0.02309216] [-0.01196764 -0.01424262 0.99982695 -0.00048804] [ 0. 0. 0. 1. ]] timeshift cam0 to imu0: [s] (t_imu = t_cam + shift) 0.005309643637061895 Gravity vector in target coords: [m/s^2] [ 0.04112907 -9.80355932 -0.23865438] Calibration configuration ========================= cam0 ----- Camera model: pinhole Focal length: [619.625632, 616.574239] Principal point: [332.100914, 246.877536] Distortion model: radtan Distortion coefficients: [0.134955, -0.247547, 0.006056, 0.002826] Type: aprilgrid Tags: Rows: 6 Cols: 6 Size: 0.022 [m] Spacing 0.006599999999999999 [m] IMU configuration ================= IMU0: ---------------------------- Model: calibrated Update rate: 200.0 Accelerometer: Noise density: 0.128 Noise density (discrete): 1.8101933598375617 Random walk: 0.0005 Gyroscope: Noise density: 0.00183 Noise density (discrete): 0.02588010819142764 Random walk: 1.94e-05 T_i_b [[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]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
四、相机固件升级
(1) 问题描述和介绍
我理解的D435i作为一个相机需要软硬件进行驱动，主要包括三个东西
- realsense-viewer SDK
- 相机固件FW
- ROS包
其中各类问题包括：陀螺仪飘、imu话题不输出等等问题，都可以通过软（realsense-viewer SDK）硬件（相机固件FW）版本配合解决。
我遇到的问题包括不限于：
（1）相机的imu的陀螺仪静止时不稳定，会大幅度随机漂移，标定陀螺仪噪声是原来的几百倍。这会导致vins在初始化的时候无法标定外参以及重力加速度;
（2）相机的imu没有话题消息更新;
（2）相关命令
1.首先将相机连接上
注意是USB3.0,蓝色的口，黑色的是USB2.0，一定要用USB3.0！！！！
接下来测试相机是否正常运行：
打开相机sdk，分别大家三个模块，双目，rgbd，imu
realsense-viewer
- 1
打卡ROS包
roscore
roslaunch realsense2_camera rs_camera.launch
rostopic hz /camera/imu
- 1
- 2
- 3
检查imu话题打印频率，200hz。
2. 指令介绍
注意第二行命令，会有推荐的固件版本，按照推荐版本下载，一般就不会有问题了。
rs-fw-update -l//查看相机的序列号，固件版本号等
rs-sensor-control//查看推荐的固件版本号
- 1
- 2
inter D435i相机固件版本下载网址：https://dev.intelrealsense.com/docs/firmware-releases-d400
通过rs-sensor-control查询到推荐的固件版本后在上面网址下载对应的固件版本压缩包，解压后
在文件下会有一个.bin文件，执行命令：
rs-fw-update -s 相机序列号 -f Signed_Image_UVC_固件版本号.bin
- 1
会显示下方信息
search for device with serial number: xxxxxxxxxx
update to FW: Signed_Image_UVC_5_11_6_250.bin
updating device:
Name: Intel RealSense D415, serial number: xxxxxxxxxx, ASIC serial number: xxxxxxxxxx, firmware version: 05.11.01.100, USB type: 3.2
firmware update started
firmware update progress: 100[%]
firmware update done
device 725112060411 successfully updated to FW: 05.11.06.250
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
看到更新到100%后提升成功即可，
至此相机固件完毕！
五、相机Imu校准
（1）为什么要校准
IMU六面校准是指校准acc、gyro的零偏、尺度因子及非正交误差。
测量类工具使用前校准是很合理的一件事情，别管为什么了，校准一下吃不了亏上不了当，有个博主说打开相机
realsense-viewer的imu模块如果加速度计的模值不是9.8左右就应该进行校准，我的相机之前就是9.6左右，校准完了就是9.8了，确实是起作用了。
（2）校准过程
校准过程是使用inter开发的自动化测量程序，管方文档下载链接：https://dev.intelrealsense.com/docs/imu-calibration-tool-for-intel-realsense-depth-camera
文档建议使用windows10、ubuntu16.04或者18.04,我的ubuntu是20.04也可以正常校准的
下载相关文件
1.将相机和电脑通过USB3.0连接
2.下载标定工具库：https://github.com/IntelRealSense/librealsense
3.下载python工具： sudo pip3 install pyrealsense2
开始校准
进入rs-imu-calibration文件夹
cd librealsense/tools/rs-imu-calibration
python rs-imu-calibration.py
- 1
- 2
注： 1.为保证六面水平，可用realsense包装盒制作六面校准工装，2.采集每一面数据时，保证无振动干扰。
->校准Python脚本中的校准算法需要设备的6个不同位置来计算校准。设备应该在每一个位置保持3到4秒。一定要在每个位置保持相机尽可能稳定。
->由于产品设计和IMU在产品中的物理配置，D435i, D455和L515之间的位置不同，但在每个位置的目标是使IMU的轴与重力方向对齐，并按以下顺序描述。这些产品在设备的底部都有¼-20螺纹的三脚架安装螺纹，所以下面的校准脚本和说明将其作为。
将用realsense包装盒做好的方块按照下面的摆放顺序进行标定：
位置1：安装螺钉指向下方，设备朝外
3秒左右相机固定好就会开始记录数据，记录完成后会一直刷消息提示更换到下一个位置，摆放正确后会倒计时3秒左右后开始记录数据，其他位置也是同理
位置2：安装螺钉向左，设备朝外：摄像机朝向上节所述的相同方向，围绕摄像机的观看方向旋转摄像机90度，使¼- 20螺纹三脚架指向左侧。
####位置3：安装螺钉指向上，设备朝向外
####位置4：安装螺钉指向右侧，设备朝外
####位置5：相机的观看方向朝下，使英特尔®RealSense™标志朝上。
####位置6：从5号位置将摄像头围绕USB线旋转180度，使英特尔®RealSense™标志朝下。
##（3）校准完成
六个位置均收集好数据后程序就会提示是否保存数据，回车保存即可
然后询问是否写入数据，Y后就会将刚才校准的结果写入相机。
至此，校准完成！！
六、D435i双目相机和IMU标定
本节基本参考博客：https://blog.csdn.net/m0_46555669/article/details/127277626
1.双目相机的标定
先重新搞一个rs_stereo_camera.launch，把里面双目相机打开，结构光关了，然后
<launch> <arg name="serial_no" default=""/> <arg name="usb_port_id" default=""/> <arg name="device_type" default=""/> <arg name="json_file_path" default=""/> <arg name="camera" default="camera"/> <arg name="tf_prefix" default="$(arg camera)"/> <arg name="external_manager" default="false"/> <arg name="manager" default="realsense2_camera_manager"/> <arg name="fisheye_width" default="640"/> <arg name="fisheye_height" default="480"/> <arg name="enable_fisheye" default="false"/> <arg name="depth_width" default="640"/> <arg name="depth_height" default="480"/> <arg name="enable_depth" default="true"/> <arg name="infra_width" default="640"/> <arg name="infra_height" default="480"/> <arg name="enable_infra1" default="true"/> <arg name="enable_infra2" default="true"/> <arg name="color_width" default="640"/> <arg name="color_height" default="480"/> <arg name="enable_color" default="true"/> <arg name="fisheye_fps" default="30"/> <arg name="depth_fps" default="30"/> <arg name="infra_fps" default="30"/> <arg name="color_fps" default="30"/> <arg name="gyro_fps" default="200"/> <arg name="accel_fps" default="200"/> <arg name="enable_gyro" default="true"/> <arg name="enable_accel" default="true"/> <arg name="enable_pointcloud" default="false"/> <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/> <arg name="pointcloud_texture_index" default="0"/> <arg name="enable_sync" default="true"/> <arg name="align_depth" default="true"/> <arg name="publish_tf" default="true"/> <arg name="tf_publish_rate" default="0"/> <arg name="filters" default=""/> <arg name="clip_distance" default="-2"/> <arg name="linear_accel_cov" default="0.01"/> <arg name="initial_reset" default="false"/> <arg name="unite_imu_method" default="linear_interpolation"/> <arg name="topic_odom_in" default="odom_in"/> <arg name="calib_odom_file" default=""/> <arg name="publish_odom_tf" default="true"/> <arg name="allow_no_texture_points" default="false"/> <arg name="emitter_enable" default="false"/> <!-- rosparam set /camera/stereo_module/emitter_enabled false --> <rosparam> /camera/stereo_module/emitter_enabled: 0 </rosparam> <rosparam if="$(arg emitter_enable)"> /camera/stereo_module/emitter_enabled: 1 </rosparam> <group ns="$(arg camera)"> <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml"> <arg name="tf_prefix" value="$(arg tf_prefix)"/> <arg name="external_manager" value="$(arg external_manager)"/> <arg name="manager" value="$(arg manager)"/> <arg name="serial_no" value="$(arg serial_no)"/> <arg name="usb_port_id" value="$(arg usb_port_id)"/> <arg name="device_type" value="$(arg device_type)"/> <arg name="json_file_path" value="$(arg json_file_path)"/> <arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/> <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/> <arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/> <arg name="enable_sync" value="$(arg enable_sync)"/> <arg name="align_depth" value="$(arg align_depth)"/> <arg name="fisheye_width" value="$(arg fisheye_width)"/> <arg name="fisheye_height" value="$(arg fisheye_height)"/> <arg name="enable_fisheye" value="$(arg enable_fisheye)"/> <arg name="depth_width" value="$(arg depth_width)"/> <arg name="depth_height" value="$(arg depth_height)"/> <arg name="enable_depth" value="$(arg enable_depth)"/> <arg name="color_width" value="$(arg color_width)"/> <arg name="color_height" value="$(arg color_height)"/> <arg name="enable_color" value="$(arg enable_color)"/> <arg name="infra_width" value="$(arg infra_width)"/> <arg name="infra_height" value="$(arg infra_height)"/> <arg name="enable_infra1" value="$(arg enable_infra1)"/> <arg name="enable_infra2" value="$(arg enable_infra2)"/> <arg name="fisheye_fps" value="$(arg fisheye_fps)"/> <arg name="depth_fps" value="$(arg depth_fps)"/> <arg name="infra_fps" value="$(arg infra_fps)"/> <arg name="color_fps" value="$(arg color_fps)"/> <arg name="gyro_fps" value="$(arg gyro_fps)"/> <arg name="accel_fps" value="$(arg accel_fps)"/> <arg name="enable_gyro" value="$(arg enable_gyro)"/> <arg name="enable_accel" value="$(arg enable_accel)"/> <arg name="publish_tf" value="$(arg publish_tf)"/> <arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/> <arg name="filters" value="$(arg filters)"/> <arg name="clip_distance" value="$(arg clip_distance)"/> <arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/> <arg name="initial_reset" value="$(arg initial_reset)"/> <arg name="unite_imu_method" value="$(arg unite_imu_method)"/> <arg name="topic_odom_in" value="$(arg topic_odom_in)"/> <arg name="calib_odom_file" value="$(arg calib_odom_file)"/> <arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/> <arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/> </include> </group> </launch>
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
打开相机：
roscore
roslaunch realsense2_camera rs_stereo_camera.launch
- 1
- 2
检查话题：/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw
开始标定
开始移动相机，同时确保标定板一直在三个图像中，录制过程参考官方操作https://www.youtube.com/watch?app=desktop
步骤如下：
（1）俯仰角摆动3次
（2）偏航角摆动3次
（3）翻滚角摆动3次
（4）上下移动3次
（5）左右移动3次
（6）前后移动3次
（7）自由移动，摆动幅度大一些，但要移动缓慢些，使得标定目标尽可能出现在相机的所有视野范围内
整体标定时间在90s以上
修改相机帧数
官方推荐4Hz，通过如下命令更改topic发布频率
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right
- 1
- 2
- 3
录制.bag文件
rosbag record -O multicameras_calibration_biaoding /infra_left /infra_right /color
- 1
/infra_left 、/infra_right、 /color为频率转换后的topic
使用kalib工具箱开始标定：
source ./devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --target checkerboard.yaml --bag multicameras_calibration_biaoding.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topic /infra_left /infra_right /color --bag-from-to 1 40 --show-extraction --approx-sync 0.04
- 1
- 2
双目标定结果：
cam0: cam_overlaps: - 1 - 2 camera_model: pinhole distortion_coeffs: - 0.0777595208930149 - -0.11522585995931633 - 0.0101885002320918 - 0.013911852704849074 distortion_model: radtan intrinsics: - 509.6431045394015 - 506.92565688455454 - 333.5279706484233 - 270.4465709227393 resolution: - 640 - 480 rostopic: /infra_left cam1: T_cn_cnm1: - - 0.9999902623144893 - 0.0013300504951655498 - 0.004207878547600591 - -0.05040262721792144 - - -0.0014048522754477348 - 0.9998401551567179 - 0.017823875175356867 - 0.00019353437741919654 - - -0.004183499285910864 - -0.0178296130598183 - 0.9998322875521944 - -0.004518850433654948 - - 0.0 - 0.0 - 0.0 - 1.0 cam_overlaps: - 0 - 2 camera_model: pinhole distortion_coeffs: - 0.07522741012636508 - -0.09383314097105816 - 0.0031686202561990906 - 0.01741885160250619 distortion_model: radtan intrinsics: - 508.17945010621895 - 504.258558843024 - 332.24269608080414 - 261.26239620277903 resolution: - 640 - 480 rostopic: /infra_right cam2: T_cn_cnm1: - - 0.9999654931104104 - 0.007992669351053154 - -0.002264911716069819 - 0.0655777954476903 - - -0.008018426820028233 - 0.9999005539411384 - -0.01160116629996367 - -0.0010076504678487015 - - 0.002171962193203815 - 0.011618927008648276 - 0.9999301391174196 - 0.006215261951084017 - - 0.0 - 0.0 - 0.0 - 1.0 cam_overlaps: - 0 - 1 camera_model: pinhole distortion_coeffs: - 0.21363586706650475 - -0.41148796205920674 - 0.013605843805251101 - 0.013364321924207974 distortion_model: radtan intrinsics: - 788.8988906688113 - 785.0700431091585 - 346.7644279565707 - 292.29525898108795 resolution: - 640 - 480 rostopic: /color
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
注：
（1）–target ~/kalibr_workspace/april_6x6_A4.yaml是标定板的配置文件
（2）–bag ~/multicameras_calibration_biaoding.bag是录制的ROS bag数据包
（3）–models pinhole-radtan pinhole-radtan pinhole-radtan表示三个摄像头的相机模型和畸变模型（VINS使用的畸变模型为radtan）
（4）–topics /infra_left /infra_right /color表示双目相机和rgb相机的话题
（5）–bag-from-to 1 40表示处理bag中1s-40s的数据（根据自己录制的数据包时间设置，rosbag info 你自己的数据包 即可查看数据包信息）
（6）–show-extraction表示显示检测特征点的过程
2. 双目相机和IMU之间的标定
（1）编写chain_biaoding.yaml文件
格式参考Kalibr官方https://github.com/ethz-asl/kalibr/wiki/yaml-formats中的chain.yaml
文件中的参数需要根据之前相机标定的参数进行修改，示例如下：
cam0: camera_model: pinhole distortion_coeffs: [0.0777595208930149,-0.11522585995931633,0.0101885002320918,0.013911852704849074] distortion_model: radtan intrinsics: [509.6431045394015,506.92565688455454,333.5279706484233,270.4465709227393] resolution: [640, 480] rostopic: /infra_left cam1: T_cn_cnm1: - [0.9999902623144893,0.0013300504951655498,0.004207878547600591,-0.05040262721792144] - [-0.0014048522754477348,0.9998401551567179,0.017823875175356867,0.00019353437741919654] - [-0.004183499285910864,-0.0178296130598183,0.9998322875521944,-0.004518850433654948] - [0.0,0.0,0.0,1.0] camera_model: pinhole distortion_coeffs: [0.07522741012636508,-0.09383314097105816,0.0031686202561990906,0.01741885160250619] distortion_model: radtan intrinsics: [508.17945010621895,504.258558843024,332.24269608080414,261.26239620277903] resolution: [640, 480] rostopic: /infra_right
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
T_cn_cnm1表示的是左目相机到右目相机的旋转和平移，参考之前相机标定的结果。标定结果里面有两个T,我用的第一个。
（2）编写imu.yaml
格式参考https://github.com/ethz-asl/kalibr/wiki/yaml-formats
中的imu.yaml，文件中的参数参考之前imu标定得到的参数，示例如下：
rostopic: /imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 1.27e-2
accelerometer_random_walk: 4.11e-4
gyroscope_noise_density: 1.78e-3
gyroscope_random_walk: 1.76e-5
- 1
- 2
- 3
- 4
- 5
- 6
（3）录制bag包
调整话题发布：
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
- 1
- 2
- 3
录制双目信息和IMU信息：
rosbag record -O imu_stereo_biaoding.bag /infra_left /infra_right /imu
- 1
（4）开始标定
source ./devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --bag imu_stereo_biaoding.bag --cam chain_biaoding.yaml --imu imu.yaml --target checkerboard.yaml --bag-from-to 1 75 --show-extraction
- 1
- 2
标定结果好坏查看results-imucam-imu_stereo_biaoding.txt中的重投影误差Reprojection error，两个相机的数值都在0.15以下说明标定结果良好
标定结果camchain-imucam-imu_stereo_biaoding.yaml如下所示:
cam0: T_cam_imu: - - 0.9998008706096181 - 0.015952816113438628 - -0.01198861069103996 - 0.006424891930366121 - - -0.01612483603633693 - 0.9997664082934555 - -0.014391612515823386 - -0.013379677262455833 - - 0.011756223502968744 - 0.014582061104492988 - 0.9998245619621944 - 0.05438345333823401 - - 0.0 - 0.0 - 0.0 - 1.0 camera_model: pinhole distortion_coeffs: - 0.0777595208930149 - -0.11522585995931633 - 0.0101885002320918 - 0.013911852704849074 distortion_model: radtan intrinsics: - 509.6431045394015 - 506.92565688455454 - 333.5279706484233 - 270.4465709227393 resolution: - 640 - 480 rostopic: /infra_left timeshift_cam_imu: 0.0024198029258919937 cam1: T_cam_imu: - - 0.999819156777695 - 0.017343760118434363 - -0.007800495195427406 - -0.043766754530854046 - - -0.017317289632267852 - 0.9998440982755161 - 0.0034482783259025393 - -0.012223706352763857 - - 0.007859085196880658 - -0.0033125712935643467 - 0.9999636301642645 - 0.05006715806015189 - - 0.0 - 0.0 - 0.0 - 1.0 T_cn_cnm1: - - 0.9999902623144911 - 0.0013300504951655498 - 0.004207878547600591 - -0.05040262721792144 - - -0.0014048522754477348 - 0.9998401551567196 - 0.01782387517535687 - 0.00019353437741919654 - - -0.004183499285910864 - -0.017829613059818304 - 0.9998322875521962 - -0.004518850433654948 - - 0.0 - 0.0 - 0.0 - 1.0 camera_model: pinhole distortion_coeffs: - 0.07522741012636508 - -0.09383314097105816 - 0.0031686202561990906 - 0.01741885160250619 distortion_model: radtan intrinsics: - 508.17945010621895 - 504.258558843024 - 332.24269608080414 - 261.26239620277903 resolution: - 640 - 480 rostopic: /infra_right timeshift_cam_imu: 0.002390682238297422
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
results-imucam-imu_stereo_biaoding.txt：如下所示
七、VINS-Fusion文件配置
根据联合标定结果中的results-imucam-imu_stereo_biaoding.txt文件修改realsense_stereo_imu_config.yaml、left.yaml和right.yaml文件。
1.realsense_stereo_imu_config.yaml文件修改
双目和IMU直接的外参修改结果如下：
结果取自results-imucam-imu_stereo_biaoding.txt中的T_ic: (cam0 to imu0)和T_ic (cam1 to imu0)
realsense_stereo_imu_config.yaml文件中外参更改如下：
body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [0.99980087,-0.01612484,0.01175622,-0.0072787, 0.01595282,0.99976641,0.01458206,0.01248103, -0.01198861,-0.01439161,0.99982456,-0.05448944, 0,0,0,1] body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [0.99981916,-0.01731729,0.00785909,0.04315368, 0.01734376,0.9998441,-0.00331257,0.01314673, -0.0078005,0.00344828,0.99996363,-0.05036459, 0,0,0,1]
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
realsense_stereo_imu_config.yaml文件中imu parameters用前面IMU标定的结果，或者imu-imu_stereo_biaoding.yaml中的，这是一样的：
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 1.27e-2 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 1.78e-3 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 4.11e-4 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 1.76e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.805 # gravity magnitude
- 1
- 2
- 3
- 4
- 5
- 6
完整realsense_stereo_imu_config.yaml如下所示：
%YAML:1.0 #common parameters #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; imu: 1 num_of_cam: 2 imu_topic: "/camera/imu" image0_topic: "/camera/infra1/image_rect_raw" image1_topic: "/camera/infra2/image_rect_raw" output_path: "/home/dji/output/" cam0_calib: "left.yaml" cam1_calib: "right.yaml" image_width: 640 image_height: 480 # Extrinsic parameter between IMU and Camera. estimate_extrinsic: 1 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it. # 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess. body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [0.99980087,-0.01612484,0.01175622,-0.0072787, 0.01595282,0.99976641,0.01458206,0.01248103, -0.01198861,-0.01439161,0.99982456,-0.05448944, 0,0,0,1] body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [0.99981916,-0.01731729,0.00785909,0.04315368, 0.01734376,0.9998441,-0.00331257,0.01314673, -0.0078005,0.00344828,0.99996363,-0.05036459, 0,0,0,1] #Multiple thread support multiple_thread: 1 #feature traker paprameters max_cnt: 150 # max feature number in feature tracking min_dist: 30 # min distance between two features freq: 10 # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image F_threshold: 1.0 # ransac threshold (pixel) show_track: 0 # publish tracking image as topic flow_back: 1 # perform forward and backward optical flow to improve feature tracking accuracy #optimization parameters max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time max_num_iterations: 8 # max solver itrations, to guarantee real time keyframe_parallax: 10.0 # keyframe selection threshold (pixel) #imu parameters The more accurate parameters you provide, the better performance acc_n: 1.27e-2 # accelerometer measurement noise standard deviation. #0.2 0.04 gyr_n: 1.78e-3 # gyroscope measurement noise standard deviation. #0.05 0.004 acc_w: 4.11e-4 # accelerometer bias random work noise standard deviation. #0.002 gyr_w: 1.76e-5 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.805 # gravity magnitude #unsynchronization parameters estimate_td: 1 # online estimate time offset between camera and imu td: 0.00 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock) #loop closure parameters load_previous_pose_graph: 0 # load and reuse previous pose graph; load from 'pose_graph_save_path' pose_graph_save_path: "/home/siae302/catkin_ws/src/VINS-Fusion/output/pose_graph/" # save and load path save_image: 1 # save image in pose graph for visualization prupose; you can close this function by setting 0
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
2. left.yaml 和right.yaml
根据camchain-imucam-imu_stereo_biaoding.yaml中的相机内参和畸变参数进行修改，cam0是左目，cam1是右目结果如下：
left.yaml:
%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: 0.0777595208930149 k2: -0.11522585995931633 p1: 0.0101885002320918 p2: 0.013911852704849074 projection_parameters: fx: 509.6431045394015 fy: 506.92565688455454 cx: 333.5279706484233 cy: 270.4465709227393
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
right.yaml
%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 640 image_height: 480 distortion_parameters: k1: 0.07522741012636508 k2: -0.09383314097105816 p1: 0.0031686202561990906 p2: 0.01741885160250619 projection_parameters: fx: 508.17945010621895 fy: 504.258558843024 cx: 332.24269608080414 cy: 261.26239620277903
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
八、运行vins-fusion
1.启动相机
roscore
roslaunch realsense2_camera rs_stereo_camera.launch
//rs_stereo_camera.launch是我专门用来运行vins-fusion的，前面有说
- 1
- 2
- 3
2.启动rviz
roslaunch vins vins_rviz.launch
- 1
3.运行vins-fusion
cpprosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
- 1
制作不易，已经看到这里的兄弟姐妹是不是应该三连一波，关注点赞收藏！
九、参考链接
https://blog.csdn.net/Hanghang_/article/details/103546033#t19
https://blog.csdn.net/qq_36170626/article/details/102998942?utm_source=app
https://blog.csdn.net/weixin_40628128/article/details/95945945?utm_source=app
https://blog.csdn.net/qq_25458977/article/details/102806768
https://blog.csdn.net/qq_41754202/article/details/121312211
https://blog.csdn.net/m0_63203209/article/details/129855794
https://blog.csdn.net/u010196944/article/details/127238908
等等，感谢大佬们的博客分享～
