ZED2相机标定及运行VINS-mono

一、ZED2相机+IMU标定

1、标定工具安装

Kalibr的安装参见我的另外一篇博客Kalibr安装及相机+IMU标定
(需要注意的是Kalibr的安装需要网络能用google,否则网络问题会导致安装不成功,如需可以找我copy相关安装所需文件)
接下来是imu_utils的安装,参考github上的安装要求即可,且依赖于code_utils,需要先安装code_utils,然后创建catkin工作空间,必须先把code_utils放进去catkin_make,然后再把imu_utils文件放入工作空间中catkin_make,否则会报错找不到code_utils。

2、ZED2标定数据录制

我根据需求修改了ZED2的分辨率,在ZED2_WS/src/zed-ros-wrapper/zed_wrapper/params文件夹下找到common.yaml,设置resolution为3,即VGA模式,实际分辨率大小为672*376.

然后打开ZED2相机开启数据录制:

roslaunch zed_wrapper zed2.launch

启用左右摄像头可视化功能,以确保将标定板保持在相机范围内。

rosrun image_view image_view image:=/zed2/zed_node/left/image_rect_color & rosrun image_view image_view image:=/zed2/zed_node/right/image_rect_color 

kalibr在处理标定数据的时候要求图像的频率不可过高,降低图像数据到20HZ,IMU数据至200HZ.

rosrun topic_tools throttle messages /zed2/zed_node/imu/data_raw  200 /zed2/zed_node/imu/data_raw2
rosrun topic_tools throttle messages /zed2/zed_node/left/image_rect_color 20 /zed2/zed_node/left/image_rect_color2
rosrun topic_tools throttle messages /zed2/zed_node/right/image_rect_color 20 /zed2/zed_node/right/image_rect_color2

录制数据

rosbag record -O Kalib_data_vga.bag /zed2/zed_node/imu/data_raw2 /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2

3、开始相机标定

这里使用的是棋盘格标定板,对应的checkboard.yaml文件内容为:

target_type: 'checkerboard' #gridtype
targetCols: 8               #number of internal chessboard corners
targetRows: 11               #number of internal chessboard corners
rowSpacingMeters: 0.03      #size of one chessboard square [m]
colSpacingMeters: 0.03      #size of one chessboard square [m]

执行标定:

单目情况

kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 --models pinhole-radtan  --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04

双目情况:

kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2 --models pinhole-radtan  pinhole-radtan --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04

左目标定结果为:

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
    2.6184399452903757e-05]
  distortion_model: radtan
  intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
  resolution: [672, 376]
  rostopic: /zed2/zed_node/left/image_rect_color2

注意:

在最初运行标定时有报错,一直卡在提取角点这一步

Initializing cam0:
    Camera model:      pinhole-radtan
    Dataset:          Kalib_data_vga.bag 
    Topic:            /zed2/zed_node/left/image_rect_color2
    Number of images: 1980
Extracting calibration target corners
  Progress 19 / 1980     Time remaining: 9m

解决办法为:

单、双目:
修改kalibr_calibrate_cameras.py文件中的多线程标签multithreading=multithreading改为multithreading=False

observations = kc.extractCornersFromDataset(cam.dataset, cam.ctarget.detector,

                                                                                  multithreading=False, clearImages=False,

                                                                                  noTransformation=True)

联合标定:

修改IccSensors.py文件中的多线程标签multithreading=multithreading改为multithreading=False

self.targetObservations = kc.extractCornersFromDataset(self.dataset, self.detector, multithreading=False)

在执行单目标定、单目+IMU联合表定时会报错如下:

Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
Traceback (most recent call last):
  File "/home/ipsg/tool/kalibr_ws/devel/bin/kalibr_calibrate_cameras", line 15, in 
    exec(compile(fh.read(), python_script, 'exec'), context)
  File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 447, in 
    main()
  File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 204, in main
    graph.plotGraph()
  File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py", line 311, in plotGraph
    edge_label=self.G.es["weight"],
KeyError: 'Attribute does not exist'

解决办法:

找到工作空间下src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras中的如下代码,然后注释掉

if not graph.isGraphConnected(): 
    obsdb.printTable() 
    print "Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance." 
    graph.plotGraph() 
    sys.exit(-1)

注释掉之后便可以得出标定结果。

修改以上信息之后,便可以顺利运行。

4、IMU参数标定

单独录制IMU数据,数据包录制我录制了两个多小时,录制过程中必须保持相机静止不动。

 rosbag record -O imu_calibration /zed2/zed_node/imu/data_raw

根据imu_utils文件夹里面的A3.launch改写ZED2标定启动文件:ZED2_calibration.launch注意,max_time_min对应的参数,默认是120,意味着两个小时,如果数据录制时间超过两小时可以不用修改,如果不足,这个时间值要改为略小于真实时间。我的内容如下:


    
        
        
        
        
        
    

启动标定:

roslaunch imu_utils ZED2_calibration.launch

回访数据包,以200Hz的速率回放:

rosbag play -r 200 imu_calibration.bag  

最后可以得到标定结果文件:/home/ipsg/tool/utils_ws/src/imu_utils/data/ZED2_imu_param.yaml

%YAML:1.0
---
type: IMU
name: ZED2
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 1.8222305208357593e-03
      gyr_w: 3.7133721747382378e-05
   x-axis:
      gyr_n: 2.1821037744825752e-03
      gyr_w: 4.3929681760916831e-05
   y-axis:
      gyr_n: 1.6623543812424751e-03
      gyr_w: 4.1416410773020793e-05
   z-axis:
      gyr_n: 1.6222334067822277e-03
      gyr_w: 2.6055072708209496e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 1.9690445544535126e-02
      acc_w: 5.2966882234280319e-04
   x-axis:
      acc_n: 2.0618609665773655e-02
      acc_w: 5.4447136705058940e-04
   y-axis:
      acc_n: 1.7485637447877407e-02
      acc_w: 6.3371577751311896e-04
   z-axis:
      acc_n: 2.0967089519954317e-02
      acc_w: 4.1081932246470108e-04

5、执行相机+IMU联合标定

利用步骤2中录制的数据包,执行标定,需要准备cam.yaml及imu.yaml文件,cam.yaml为单双目输出的标定文件,本次标定仅对左目标定,使用如下:

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
    2.6184399452903757e-05]
  distortion_model: radtan
  intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
  resolution: [672, 376]
  rostopic: /zed2/zed_node/left/image_rect_color2

imu.yaml信息由步骤4中的IMU标定结果得出,取标定结果Acc及Gyr的平均值填入imu.yaml文件,得如下内容:

#Accelerometers
accelerometer_noise_density: 1.96e-02   #Noise density (continuous-time)
accelerometer_random_walk:   5.29e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.82e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.71e-05   #Bias random walk

rostopic:                    /zed2/zed_node/imu/data_raw2   #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

除了使用自己标定的IMU参数信息,也可以直接使用官网上提供的该参数,经验证,标定过后也是可以在系统中运行的,其给定的参数为(IMU rate及topic需要修改):

#Accelerometers
accelerometer_noise_density: 1.4e-03   #Noise density (continuous-time)
accelerometer_random_walk:   8.0e-05   #Bias random walk
 
#Gyroscopes
gyroscope_noise_density:     8.6e-05   #Noise density (continuous-time)
gyroscope_random_walk:       2.2e-06   #Bias random walk
 
rostopic:                    /zed2/zed_node/imu/data_raw      #the IMU ROS topic
update_rate:                 400.0     #Hz (for discretization of the values above)

执行联合标定:

kalibr_calibrate_imu_camera \
    --target /home/ipsg/dataset/checkboard.yaml \
    --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag \
    --bag-from-to 10 130 \
    --cam /home/ipsg/tool/kalibr_ws/cam.yaml \
    --imu /home/ipsg/tool/kalibr_ws/imu.yaml \
    --imu-models scale-misalignment \
    --timeoffset-padding 0.1

可得标定结果为camchain-imucam-homeipsgZED2_WSKalib_data_vga.yaml,如下:

cam0:
  T_cam_imu:
  - [0.010461959926441555, -0.9997613415836004, -0.019178302048279805, 0.0261061220321363]
  - [-0.04841582739273925, 0.01865039796109419, -0.9986531281249599, 0.004580655437468762]
  - [0.9987724741162884, 0.011376402368514227, -0.04820915283196389, -0.06740069267783835]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
    2.6184399452903757e-05]
  distortion_model: radtan
  intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
  resolution: [672, 376]
  rostopic: /zed2/zed_node/left/image_rect_color2
  timeshift_cam_imu: 0.001248879099353284

二、使用ZED2运行VINS-mono

修改realsense_color_config.yaml文件
1、订阅topics修改

imu_topic: "/zed2/zed_node/imu/data_raw"
image_topic: "/zed2/zed_node/left/image_rect_color"

2、左目相机内参修改

model_type: PINHOLE
camera_name: camera
image_width: 672
image_height: 376
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 264.154447
   fy: 265.630308
   cx: 357.121886
   cy: 167.479994

这里使用的是校正后的图像,故设置畸变系数均为0;

3、IMU至cam的变换矩阵,参数修改为2,使用在线标定(设置为0,使用已有的标定参数也是可以运行的):

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 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.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

4、IMU参数,使用VINS-mono中给定的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

5、不需要在线估计同步时差

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

6、相机改为全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

7、开始运行

roslaunch zed_wrapper zed2.launch
roslaunch vins_estimator realsense_live.launch 
roslaunch vins_estimator vins_rviz.launch

结果如下:
在这里插入图片描述

参考链接:
https://blog.csdn.net/u011178262/article/details/83316968
https://blog.csdn.net/fb_941219/article/details/104709049
https://blog.csdn.net/u010590316/article/details/89297324
https://zhuanlan.zhihu.com/p/268825840
https://blog.csdn.net/weixin_44580210/article/details/89789416

你可能感兴趣的:(Ubuntu,ZED2相机标定,ZED2,VINS)