ZED双目相机标定跑通vins fusion

一、环境

1.所需工具依赖及环境

ubuntu18.04
ros1
code_utils
imu_utils
Kalibr
zed-ros-wrapper
vins fusion（能够运行）

2.相机

ZED 2
链接: ZED 2

二、相机标定

1.标定板准备

链接: 标定板

• 下载Aprilgrid 6x6 0.8x0.8 m (A0 page)两个文件
  

• 用A4纸打印pdf文件，量好大正方形和小正方形的尺寸，我这里量的尺寸是2cm和0.6cm

• 修改yaml文件，tagSize是大正方形尺寸，tagSpacing是小正方形/大正方形尺寸（单位是m）
  

• 用zed 2相机采集ros包，可参考下面的视频
  https://www.youtube.com/watch?app=desktop&v=puNXsnrYWTY
  （1）俯仰角摆动3次
  （2）偏航角摆动3次
  （3）翻滚角摆动3次
  （4）上下移动3次
  （5）左右移动3次
  （6）前后移动3次
  （7）自由移动，摆动幅度大一些，但要移动缓慢些，使得标定目标尽可能出现在相机的所有视野范围内
  整体标定时间在90s以上
  具体步骤如下

• 链接zed 2相机

• 打开zed-ros-wrapper文件，根据自己需求修改文件夹及频率

source devel/setup.bash

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 

##限制发布频率
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

ctrl+c终止录制，在当前目录下可以看到自己的录制bag包

• 将两个文件单独放置一个文件夹（我这里放置在了kalibr的文件夹下）
  通过命令rosbag info+包名可以查看bag的相关属性，我这里最后把bag文件修改名称zed.bag

• 利用kalibr运行标定命令（修改自己的目录）

# 相机标定
source /home/jia/Desktop/package/kalibr_workspace/devel/setup.bash

#双目
rosrun kalibr kalibr_calibrate_cameras --bag /home/jia/Desktop/package/kalibr_workspace/zed/zed.bag --topics /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2 --models pinhole-radtan  pinhole-radtan --target /home/jia/Desktop/package/kalibr_workspace/zed/april_6x6_80x80cm.yaml --bag-from-to 5 150 --show-extraction --approx-sync 0.04

• 一段时间等待后，出现三个文件，标定结束
  

• 可以查看一下生成的txt文件中的reprojection error（重投影误差）是多少，理想范围是0.1-0.2
  

• 生成的yaml文件就是两个相机的内参及两个相机之间的变换矩阵
  

三、IMU标定

## 数据采集
source devel/setup.bash

roslaunch zed_wrapper zed2.launch

## 单独录制imu
rosbag record -O zed-imu-calibrate.bag /zed2/zed_node/imu/data_raw

• 静止zed相机，采集数据，一般为2h左右
  
• 编写一个launch文件，启动标定工具。根据自己情况进行修改

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <!-- 数据集的话题 -->
        <param name="imu_topic" type="string" value= "/zed2/zed_node/imu/data_raw"/>
        <!-- IMU的名字，后面生成的标定文件会附带这个名字作为标记 -->
        <param name="imu_name" type="string" value= "zed_imu"/>
        <!-- 标定结果输出路径 -->
        <param name="data_save_path" type="string" value= "/home/jia/Desktop/package/imu_utils/zed_imu/result"/>
        <!-- 数据集的长度，单位：分钟 -->
        <param name="max_time_min" type="int" value= "120"/>
        <!-- Allan方差的cluster，一般设置100即可 -->
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

• 将该文件放置imu_utils的launch文件夹下
  
• 运行命令：

source devel/setup.bash


roslaunch imu_utils zed_imu.launch


# 新打开一个终端 播放imu
rosbag play -r 200 /home/jia/Desktop/package/imu_utils/zed_imu/zed-imu-calibrate.bag

• 最终得到的标定文件如图所示：
  

四、Camera-IMU标定

准备好之前的相机标定的yaml文件

准备好imu的标定文件，将数据中的avg-axis数据按下面的格式写入（注意Acc与Gyr的顺序与imu的话题）

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

#Gyroscopes
gyroscope_noise_density:     1.6947414041138789e-03   #Noise density (continuous-time)
gyroscope_random_walk:       3.9106036886980679e-06  #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)

准备好之前的april_6x6_A4.yaml

准备好之前双目标定的bag文件

运行（根据自己目录修改）

source /home/jia/Desktop/package/kalibr_workspace/devel/setup.bash

# imu+双目
rosrun kalibr kalibr_calibrate_imu_camera --bag /home/jia/Desktop/package/kalibr_workspace/imu_zed/zed.bag --target /home/jia/Desktop/package/kalibr_workspace/imu_zed/april_6x6_80x80cm.yaml --cam /home/jia/Desktop/package/kalibr_workspace/imu_zed/zed-camchain.yaml --imu /home/jia/Desktop/package/kalibr_workspace/imu_zed/resultzed_imu_imu_param.yaml

运行完成后得到结果如下：

• 一般情况两个相机的数值都在0.15以下说明标定结果良好，这里稍微大点，看看后面vins fusion轨迹会不会飘
  

五、vins fusion文件修改

左相机配置文件cam0.yaml

这里我订阅的是畸变纠正后的话题，所以这里上面设置为0

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 360
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 261.15583086682227
   fy: 262.8592818059595
   cx: 319.313424543224
   cy: 184.9957507446628

右相机配置文件cam1.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 360
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 261.3180456562997
   fy: 262.65700899343017
   cx: 318.761077383217
   cy: 183.7825567322951

新建zed配置文件zed2_stereo_config.yaml

注意：这里配置文件中的body_T_cam0: !!opencv-matrix和body_T_cam1: !!opencv-matrix是相机到IMU的变换矩阵，而我们的标定结果是IMU到相机的变换矩阵，所以需要取逆
对两个矩阵求逆

这里给出一个求逆的简单代码：
main.cpp

#include 
#include 

int main() {
    Eigen::Matrix4d C_T_I; // 相机到IMU的变换矩阵

    C_T_I <<0.020061355664019148, -0.999797188647717, 0.0017673655658106413, -0.10109211728531109,
          0.008239060356727057, -0.0016023415625087134, -0.9999647745725624, -0.0033085611164389494,
          0.9997648022876982, 0.02007521042576005, 0.008205244237773884, -0.002311614462540171,
          0, 0, 0, 1;


    Eigen::Matrix4d I_T_C = C_T_I.inverse(); // 计算IMU到相机的变换矩阵

    std::cout << "Camera to IMU Transformation Matrix:\n" << C_T_I << "\n\n";
    std::cout << "IMU to Camera Transformation Matrix:\n" << I_T_C << "\n";

    return 0;
}

camkelists.txt

cmake_minimum_required(VERSION 3.12)
project(MatrixInverseExample)

set(CMAKE_CXX_STANDARD 11)

# 查找Eigen库
find_package(Eigen3 REQUIRED)

# 添加可执行文件
add_executable(matrix_inverse main.cpp)

# 链接Eigen库
target_link_libraries(matrix_inverse Eigen3::Eigen)

最终结果：

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

#实时相机
imu_topic: "/zed2/zed_node/imu/data_raw"
image0_topic: "/zed2/zed_node/left/image_rect_gray"
image1_topic: "/zed2/zed_node/right/image_rect_gray"

# 录制bag包
#imu_topic: "/zed2/zed_node/imu/data_raw2"
#image0_topic: "/zed2/zed_node/left/image_rect_color2"
#image1_topic: "/zed2/zed_node/right/image_rect_color2"
output_path: "~"

cam0_calib: "cam0.yaml"
cam1_calib: "cam1.yaml"
image_width: 640
image_height: 360
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 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.0178199, 0.00393081, 0.999833, 0.00227199,
          -0.999839, -0.00199068, 0.0178278, 0.0173767,
           0.00206042, -0.99999, 0.0038947, -0.00345778,
           0, 0, 0, 1]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.0200614, 0.00823906, 0.999765, 0.00436638,
          -0.999797, -0.00160234, 0.0200752, -0.101031,
          0.00176737, -0.999965, 0.00820524, -0.00311081,
          0, 0, 0, 1]

#Multiple thread support
multiple_thread: 0

#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: 1           # 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: 0.1          # accelerometer measurement noise standard deviation. 
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     
acc_w: 0.001        # accelerometer bias random work noise standard deviation.  
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     
g_norm: 9.81007     # gravity magnitude

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.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: "~/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 

运行vins fusion （根据自己文件修改）

./run.sh

# run.sh文件


#!/bin/bash

# Start RViz
gnome-terminal -- bash -c "source devel/setup.bash && roslaunch vins vins_rviz.launch"

# Start VINS-Fusion node
sleep 5
gnome-terminal -- bash -c "source devel/setup.bash && rosrun vins vins_node src/config/zed/zed2_stereo_config.yaml"



# Play rosbag
sleep 5
# gnome-terminal -- bash -c "source devel/setup.bash && rosbag play /home/jia/Desktop/data/EuRoc/MH_01_easy.bag"

#回环检测
sleep 5
gnome-terminal -- bash -c "source devel/setup.bash && rosrun loop_fusion loop_fusion_node src/config/zed/zed2_stereo_config.yaml"


## 实时相机
gnome-terminal -- bash -c "source devel/setup.bash && source /home/jia/Desktop/package/zed-ros-wrapper/devel/setup.bash && roslaunch zed_wrapper zed2.launch"

# Keep the terminal open until you manually close it
echo "Press Enter to close the terminals"
read

运行结果