本机环境介绍
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"/>
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
（可以将对应的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
（4）然后再下载imu_utils并编译
cd ~/catkin_imu_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ~/catkin_imu_ws/src/
catkin_make
注：（3）和（4）注意先后顺序，一个一个编译，中间有点小问题，很简单的那种，一个是头文件可能要加一下路径，还有一个要包含一下vector这个头文件。
3.编写参数配置文件
cd ~/catkin_imu_ws/src/imu_utils/launch
touch d435i_imu_calibration.launch
gedit d435i_imu_calibration.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= "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>
注：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打印频率
一切正常后不要移动相机，静置，进入想要保存bag的目录下调用终端开始录制。
rosbag record -O imu_calibration /camera/imu
至少录制你刚刚设定的时间以上(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
再调一个终端
source ~/catkin_imu_ws/devel/setup.sh
cd ~/catkin_imu_ws //如果数据包在这个文件夹下
rosbag play -r 200 imu_calibration.bag
执行完毕后即可在这个目录下/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
等下只会用到其中几个参数用于相机和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
二、相机的标定
打开d435i相机是可以通过一个话题名称查看出厂值的，可以用于和标定结果对比
roscore
roslaunch realsense2_camera rs_camera.launch
rostopic echo /camera/color/camera_info
打印如下信息：
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
---
但是其中的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
注：其中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
将标定结果存入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]
其中的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
生成如下图片，打印出来即可：
则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
（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
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
（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]
（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
注： 将/camera/color/image_raw重新发布频率为20HZ的图像话题/color，IMU同理重新发布200HZ频率话题名为/imu
此时明白为什么camchain.yaml和imu.yaml文件中的话题名称不是相机发布的话题名称了吧。
还有一件事是相机的IMU不一定能够正常发布话题，可以用进行测试，不正常就插拔一下相机。
rostopic hz /camera/imu
2.录制ros包
rosbag record -O d435i /color /imu
注意：录制的时候相机缓慢移动，并且保证标定板不要出画，同时对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.-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
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) 问题描述和介绍
我理解的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
打卡ROS包
roscore
roslaunch realsense2_camera rs_camera.launch
rostopic hz /camera/imu
检查imu话题打印频率，200hz。
2. 指令介绍
注意第二行命令，会有推荐的固件版本，按照推荐版本下载，一般就不会有问题了。
rs-fw-update -l//查看相机的序列号，固件版本号等
rs-sensor-control//查看推荐的固件版本号
inter D435i相机固件版本下载网址：https://dev.intelrealsense.com/docs/firmware-releases-d400
通过rs-sensor-control查询到推荐的固件版本后在上面网址下载对应的固件版本压缩包，解压后
在文件下会有一个.bin文件，执行命令：
rs-fw-update -s 相机序列号 -f Signed_Image_UVC_固件版本号.bin
会显示下方信息
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
看到更新到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.为保证六面水平，可用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>
打开相机：
roscore
roslaunch realsense2_camera rs_stereo_camera.launch
检查话题：/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
录制.bag文件
rosbag record -O multicameras_calibration_biaoding /infra_left /infra_right /color
/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
双目标定结果：
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）–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
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
（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
录制双目信息和IMU信息：
rosbag record -O imu_stereo_biaoding.bag /infra_left /infra_right /imu
（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
标定结果好坏查看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
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]
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
完整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
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
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
八、运行vins-fusion
1.启动相机
roscore
roslaunch realsense2_camera rs_stereo_camera.launch
//rs_stereo_camera.launch是我专门用来运行vins-fusion的，前面有说
2.启动rviz
roslaunch vins vins_rviz.launch
3.运行vins-fusion
cpprosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
制作不易，已经看到这里的兄弟姐妹是不是应该三连一波，关注点赞收藏！
九、参考链接
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
等等，感谢大佬们的博客分享～
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
