Realsence D455标定并运行Vins-Fusion
文章目录
- 一、双目相机标定
- 1. 标定板准备
- 1.1 打印标定板
- 1.2 标定板信息
- 原始pdf的格子参数是:
- 调整后的格子参数是:
- 2. 左右目相机数据准备
- 2.1 修改rs_camera.launch内容
- 2.2 关闭结构光
- 2.3 可视化双目图像
- 2.4 修改相机播包帧数
- 2.5 录制数据
- 3. kalibr 程序标定
- 4. kalibr 标定结果
- 二、IMU标定
- 1. 标定工具准备
- 1.1 code_utils
- 问题1 catkin_make时出现libdw.h没有找到
- 问题2 catkin_make时出现backward.hpp没有找到
- 1.2 imu_utils
- 2. 录制数据准备
- 2.1 编辑IMU的话题
- 方式:
- 原因:
- 最后运行:
- 2.2 录制数据
- 3. 正式标定工作
- 3.1 imu_util 工具包launch文件编写
- 3.2 imu_util标定程序运行
- 3.3 播放刚才录制的IMU数据包
- 3.4 标定结果
- 三、IMU+双目相机标定
- 1. 准备yaml文件
- 1.1 编写chain.yaml
- 1.2 编写imu.yaml
- 1.3 april_6x6_A4.yaml
- 1.4 rs_camera.launch
- 2. 录制数据
- 2.1 关闭结构光
- 2.2 可视化双目图像
- 2.3 修改相机播包帧数
- 2.4 录制数据
- 3. kalibr 程序标定
- 4. 标定结果
- 四、修改VINS的yaml文件
- 1. realsense_stereo_imu_config.yaml
- 2 left.yaml
- 3 right.yaml
- 五、 运行VINS-FUSION
- 参考
一、双目相机标定
1. 标定板准备
1.1 打印标定板
地址:https://github.com/ethz-asl/kalibr/wiki/downloads
下载,然后直接用A4纸就可以打印出来(有条件去淘宝买一个视觉标定板,标出来的误差会更小)
1.2 标定板信息
原始pdf的格子参数是:
调整后的格子参数是:
新建april_6x6_A4.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.3 # 小格子与大格子边长比例 codeOffset: 0 #code offset for the first tag in the aprilboard
千万要自己测量大格子边长,即tagSize
2. 左右目相机数据准备
在这之前需要安装realsence的sdk和ros,参考博客:Intel Realsense D455深度相机的标定及使用(一)——安装librealsense SDK2.0以及realsense-ros
2.1 修改rs_camera.launch内容
(1)打开双目
(2)打开imu的加速度计和陀螺仪
并合并为一个topic
(3)传感器同步
当然也可以修改图像分辨率(我选择默认了):
rs_camera.launch 整体
2.2 关闭结构光
默认开始IR结构光时,双目图像会有很多点,这些点可能对标定有影响,所以使用时需要关闭结构光。
终端1:
source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch
再打开一个终端2:
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled设置为off(0) ,展示如下:
2.3 可视化双目图像
新打开终端,运行rviz
rviz
修改Fiexd Fram 为camera_link
之后在里面add rgb和双目对应的topic,/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw展示如下:
之后固定标定版,尝试移动标定板,同时要确保标定板一直在三个图像当中。录制过程参考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop
需要科学上网观看
总结下来就是偏航角左右摆动3次,俯仰角摆动3次,滚转角摆动3次,上下移动3次,左右移动3次,前后移动3次,然后自由移动一段时间,摆动幅度要大一点,让视角变化大一点,但是移动要缓慢一点,同时要保证标定板在3个相机视野内部,整个标定时间要在90s以上更好。
2.4 修改相机播包帧数
修改相机帧数(官方推荐是4Hz减少冗余帧,加快计算速度)
不过有时候updatesize之后发现两个双目时间辍总是差点东西:
kalibr在处理标定数据的时候要求频率不能太高,一般为4Hz,我们可以使用如下命令来更改topic的频率,实际上是将原来的topic以新的频率转成新的topic,实际测试infra1才是对应左目相机。
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
2.5 录制数据
rosbag record -O multicameras_calibration /infra_left /infra_right /color
multicameras_calibration.bag为录制数据
3. kalibr 程序标定
这里需要安装kalibr
source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash cd /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace rosrun kalibr kalibr_calibrate_cameras \ --target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \ --bag /home/lessle6/multicameras_calibration.bag \ --models pinhole-radtan pinhole-radtan pinhole-radtan \ --topics /infra_left /infra_right /color \ --bag-from-to 3 202 --show-extraction --approx-sync 0.04
参数解释:
-
–target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml
是标定板的配置文件,注意如果选择棋格盘,注意targetCols和targetRows表示的是内侧角点的数量,不是格子数量。
-
–bag /home/lessle6/multicameras_calibration.bag \ 是录制的数据包
-
models pinhole-radtan pinhole-radtan pinhole-radtan表示三个摄像头的相机模型和畸变模型(解释参考https://github.com/ethz-asl/kalibr/wiki/supported-models,根据需要选取,其中VINS使用的畸变模型是radtan)
-
—topics /infra_left /infra_right /color表示三个摄像头对应的拍摄的数据话题
-
–bag-from-to 3 202表示处理bag中3-202秒的数据。
-
–show-extraction表示显示检测特征点的过程,这些参数可以相应的调整。
可以使用rosbag info 来参看录制的包的信息
报错1:
Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
应该是两个相机时间不同步导致的,需要调整参数:–approx-sync
参考链接:https://github.com/shanmo/Kalibr-camera-calibration/tree/master/stereo%20camera%20calibration/preliminary%20test
调整后:
rosrun kalibr kalibr_calibrate_cameras \ --target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \ --bag /home/lessle6/multicameras_calibration.bag \ --models pinhole-radtan pinhole-radtan pinhole-radtan \ --topics /infra_left /infra_right /color \ --bag-from-to 3 202 --show-extraction --approx-sync 0.1
报错2:
Initialization of focal length failed. You can enable manual input by setting ‘KALIBR_MANUAL_FOCAL_LENGTH_INIT’.
[ERROR] [1668944382.174500]: initialization of focal length for cam with topic /color failed
解决:
如果提示不能得到初始焦距的时候,可以设置:export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1(终端输入)。然后运行程序,当程序运行失败的时候,它会提示要你手动输入一个焦距,Initialization of focal length failed. Provide manual initialization: 这时你手动输入比如 400,给比较大的值,也能收敛。
如果只标定单目:
录制
rosbag record -O mono_calibration /infra_left
标定:
source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash cd /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace rosrun kalibr kalibr_calibrate_cameras \ --target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \ --bag /home/lessle6/mono_calibration.bag \ --models pinhole-radtan \ --topics /infra_left \ --show-extraction
注意: \前要有空格
4. kalibr 标定结果
如下三个文件:
二、IMU标定
1. 标定工具准备
code_utils和imu_utils都是imu标定需要用到的,用于标定imu噪声密度以及随机游走系数。 imu_utils依赖code_utils,所以先编译code_utils。
1.1 code_utils
mkdir -p /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src catkin_init_workspace cd .. catkin_make source /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/devel/setup.bash
注意“/home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws”是我的绝对ROS空间地址。 下面下载编译code_utils:
cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src git clone git@github.com:gaowenliang/code_utils.git cd .. catkin_make
问题1 catkin_make时出现libdw.h没有找到
sudo apt-get install libdw-dev
问题2 catkin_make时出现backward.hpp没有找到
解决方法:将sumpixel_test.cpp中# include "backward.hpp"改为:#include “code_utils/backward.hpp
1.2 imu_utils
cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src git clone git@github.com:gaowenliang/imu_utils.git cd .. catkin_make
2. 录制数据准备
2.1 编辑IMU的话题
方式:
找到realsense-ros包,进入 /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch(路径仅供参考), 复制一份rs_camera.launch,在rs_camera.launch里面修改:
修改为:
原因:
将accel和gyro的数据合并得到imu话题,如果不这样做发布的topic中只要加速计和陀螺仪分开的topic, 没有合并的camera/imu topic。
最后运行:
source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch
这样就可以播放对应格式的IMU。
2.2 录制数据
rosbag record -O imu_calibration /camera/imu
其中imu_calibration是bag包的名字,可以更改,录的包在当前终端目录下,/camera/imu是发布的IMU topic,可以通过以下命令查看。
rostopic list
3. 正式标定工作
3.1 imu_util 工具包launch文件编写
# 进入文件夹 cd /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src/imu_utils/launch # 创建文件 touch d455_imu_calibration.launch # 编辑文件 gedit d455_imu_calibration.launch
文件内容:
3.2 imu_util标定程序运行
source /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/devel/setup.bash roslaunch /home/lessle6/Project/1Ahahahgraduation/IMU/imu_calib_ws/src/imu_utils/launch/d455_imu_calibration.launch
3.3 播放刚才录制的IMU数据包
cd 存放imu_calibration.bag的路径 rosbag play -r 200 imu_calibration.bag
3.4 标定结果
标定结束后在imu_catkin_ws/src/imu_utils/data中生成许多文件,其中d455_imu_param.yaml就是我们想要的结果,展示如下:
%YAML:1.0 --- type: IMU name: d455 Gyr: unit: " rad/s" avg-axis: gyr_n: 1.8351398172861977e-03 gyr_w: 1.3154828587252936e-05 x-axis: gyr_n: 1.8848941703928685e-03 gyr_w: 1.7010312342829289e-05 y-axis: gyr_n: 2.3497663951613673e-03 gyr_w: 1.2699656085618862e-05 z-axis: gyr_n: 1.2707588863043573e-03 gyr_w: 9.7545173333106557e-06 Acc: unit: " m/s^2" avg-axis: acc_n: 1.7574789006499388e-02 acc_w: 5.3103238396236881e-04 x-axis: acc_n: 1.1838936933010856e-02 acc_w: 4.1400494167342325e-04 y-axis: acc_n: 1.9827984613668935e-02 acc_w: 6.3400755296148608e-04 z-axis: acc_n: 2.1057445472818376e-02 acc_w: 5.4508465725219715e-04
三、IMU+双目相机标定
1. 准备yaml文件
格式参考Kalibr官方教程: https://github.com/ethz-asl/kalibr/wiki/yaml-formats
1.1 编写chain.yaml
参考一、双目相机标定标定结果multicameras_calibration-camchain.yaml,编写chain.yaml:
cam0: camera_model: pinhole distortion_coeffs: [0.007017020579074508, 0.013075992589794715, -0.0037765402744058983, -0.0005132729806830811] distortion_model: radtan intrinsics: [437.44398421645786, 437.72233141976125, 430.95314113824475, 231.60352693067642] resolution: [848, 480] rostopic: /infra_left cam1: T_cn_cnm1: - [0.9999952197233123, 0.002108064461866742, -0.0022619891120047486, -0.09575399583929174] - [-0.0021145367376279966, 0.9999936666664111, -0.002862754171397105, 0.0005322182496990349] - [0.0022559399157413514, 0.002867523545717535, 0.9999933439997514, -0.0035089144049743597] - [0.0, 0.0, 0.0, 1.0] camera_model: pinhole distortion_coeffs: [0.018914793505132418, -0.0026985776594766744, -0.0025567868843695152, 0.00645205341789554] distortion_model: radtan intrinsics: [432.2163360247922, 431.7881804665646, 433.3636493084969, 232.14028787830168] resolution: [848, 480] rostopic: /infra_right
T_cn_cnm1:表示的是左目相机到右目相机的旋转和平移,具体的参数可以参考前面标定得到的结果
1.2 编写imu.yaml
参考二、IMU标定标定结果d455_imu_param.yaml,编写imu.yaml:
#Accelerometers accelerometer_noise_density: 1.7574789006499388e-02 #Noise density (continuous-time) accelerometer_random_walk: 5.3103238396236881e-04 #Bias random walk #Gyroscopes gyroscope_noise_density: 1.8351398172861977e-03 #Noise density (continuous-time) gyroscope_random_walk: 1.3154828587252936e-05 #Bias random walk rostopic: /imu #the IMU ROS topic update_rate: 200.0 #Hz (for discretization of the values above)
1.3 april_6x6_A4.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.3 # 小格子与大格子边长比例 codeOffset: 0 #code offset for the first tag in the aprilboard
1.4 rs_camera.launch
复制realsense-ros包中rs_camera.launch,更改内容为:
(1) imu和双目数据时间对齐
(2)合并加速计和陀螺仪的topic
2. 录制数据
2.1 关闭结构光
首先关闭IR:
默认开始IR结构光时,双目图像会有很多点,这些点可能对标定有影响,所以使用时需要关闭结构光。
终端1:
source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch
再打开一个终端2:
rosrun rqt_reconfigure rqt_reconfigure
打开后将camera->stereo_module中的emitter_enabled设置为off(0) 。
2.2 可视化双目图像
新打开终端,运行rviz
rviz
修改Fiexd Fram 为camera_link
之后在里面add rgb和双目对应的topic,/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw展示如下:
录制过程参考https://www.youtube.com/watch?v=puNXsnrYWTY&app=desktop,同样注意双目图像在整个过程要包含整个标定板,同时运动不能太快,这样会造成图像过于模糊,在前后左右上下方向来回移动,录制大概90s以上。
2.3 修改相机播包帧数
调整imu和双目topic的发布频率以及以新的topic名发布它们,其中双目图像的发布频率改为20Hz(注意这里是20了,刚才双目标定是4hz),imu发布频率改为200Hz
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
2.4 录制数据
rosbag record -O imu_stereo.bag /infra_left /infra_right /imu
imu_stereo.bag为录制数据
3. kalibr 程序标定
source /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/devel/setup.bash cd /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace rosrun kalibr kalibr_calibrate_imu_camera \ --bag /home/lessle6/imu_stereo.bag \ --cam /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/chain.yaml \ --imu /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/imu.yaml \ --target /home/lessle6/Project/1Ahahahgraduation/Calib/kalibr_workspace/data/april_6x6_A4.yaml \ --bag-from-to 2 146 \ --show-extraction
4. 标定结果
四、修改VINS的yaml文件
1. 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/zj/output/" cam0_calib: "left.yaml" cam1_calib: "right.yaml" image_width: 848 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. #相机到imu的变换矩阵 body_T_cam0: !!opencv-matrix rows: 4 cols: 4 dt: d data: [ 0.99981999, 0.00163688 , 0.0189025 , -0.00089318, -0.00146025 , 0.99995518 ,-0.00935466 ,0.00021664, -0.01891696, 0.00932537 , 0.99977757 ,0.00065183, 0. , 0. , 0. , 1. ] body_T_cam1: !!opencv-matrix rows: 4 cols: 4 dt: d data: [0.99977591, -0.0005314 , 0.0211626, 0.0949139 , 0.00066889 , 0.99997871, -0.00649049, -0.00027429, -0.0211587 , 0.00650319 , 0.99975498 , 0.0021304 , 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.7574789006499388e-02 # accelerometer measurement noise standard deviation. #0.2 0.04 gyr_n: 1.8351398172861977e-03 # gyroscope measurement noise standard deviation. #0.05 0.004 acc_w: 5.3103238396236881e-04 # accelerometer bias random work noise standard deviation. #0.002 gyr_w: 1.3154828587252936e-05 # gyroscope bias random work noise standard deviation. #4.0e-5 g_norm: 9.78921469 # gravity magnitude #unsynchronization parameters estimate_td: 1 # online estimate time offset between camera and imu td:0 # 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/zj/output/pose_graph/" # save and load path save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 0
2 left.yaml
%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 848 image_height: 480 distortion_parameters: k1: 0.007017020579074508 k2: 0.013075992589794715 p1: -0.0037765402744058983 p2: -0.0005132729806830811 projection_parameters: fx: 437.44398421645786 fy: 437.72233141976125 cx: 430.95314113824475 cy: 231.60352693067642
3 right.yaml
%YAML:1.0 --- model_type: PINHOLE camera_name: camera image_width: 848 image_height: 480 distortion_parameters: k1: 0.018914793505132418 k2: -0.0026985776594766744 p1: -0.0025567868843695152 p2: 0.00645205341789554 projection_parameters: fx: 432.2163360247922 fy: 431.7881804665646 cx: 433.3636493084969 cy: 232.14028787830168
五、 运行VINS-FUSION
启动vins
source /home/lessle6/Project/Vins-Fusion-ws/devel/setup.bash rosrun loop_fusion loop_fusion_node /home/lessle6/Project/Vins-Fusion-ws/src/VINS-Fusion/config/realsense-d455/realsense_stereo_imu_config.yaml rosrun vins vins_node /home/lessle6/Project/Vins-Fusion-ws/src/VINS-Fusion/config/realsense-d455/realsense_stereo_imu_config.yaml
启动rviz
source /home/lessle6/Project/Vins-Fusion-ws/devel/setup.bash cd /home/lessle6/Project/Vins-Fusion-ws roslaunch vins vins_rviz.launch
启动相机
source /home/lessle6/Packages/RealsenseRos_ws/devel/setup.bash roslaunch /home/lessle6/Packages/RealsenseRos_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch
参考
https://blog.csdn.net/xiaoxiaoyikesu/article/details/105646064
https://blog.csdn.net/qq_40186909/article/details/113104595?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522166312286216782391821994%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=166312286216782391821994&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2allfirst_rank_ecpm_v1~pc_rank_v39-2-113104595-null-null.142%5Ev47%5Epc_rank_34_2,201%5Ev3%5Econtrol_2&utm_term=realsense%20%E7%9A%84%20D%20%E7%9F%A9%E9%98%B5&spm=1018.2226.3001.4187
-
- 参考