当前位置：首页 > news >正文

gazebo仿真中对无人机集成的相机进行标定（VINS-Fusion）

news 来源：原创 2025/6/14 6:40:55

集成D435i和px4无人机、安装VINS-Fusion

PX4 + D435i 进行gazebo仿真
Ubuntu安装Vins-Fusion(2) —— Ubuntu20.04安装vins-fusion

设置一下参数前需进行备份

设置相机内参

启动无人机仿真，查看ros话题rostopic list
请添加图片描述
查看相机内参（仿真相机的左右目参数是一样的）

rostopic echo /camera/infra1/camera_info

请添加图片描述

#	根据下面这个矩阵填写相机的内参
#    K = [ fx  0   cx ]
#        [ 0   fy  cy ]
#        [ 0   0    1 ]
#
#	fx: 319.9348449707031
#  	fy: 319.9348449707031
#   cx: 320.0
#	cy: 240.0

right.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:k1: 0.0k2: 0.0p1: 0.0p2: 0.0
projection_parameters:fx: 319.9348449707031fy: 319.9348449707031cx: 320.0cy: 240.0

设置相机外参

设置VINS话题输入

rostopic list中无人机或相机的imu话题、相机左右目话题
请添加图片描述

相机外参（gazebo中的imu很飘，不建议使用双目 + IMU进行定位）

打开无人机gazebo仿真、打开rviz、启用VINS核心节点

添加tf

请添加图片描述

查看相对坐标

这里使用的是无人机的imu，故而使用body、camera计算外参
请添加图片描述

显示偏移和四元数

rosrun tf tf_echo body camera

请添加图片描述

通过偏移量和四元数计算外参矩阵

注：此处的四元数q = [x、y、z、w]
PS：可以直接找AI算，输入一下信息进行计算

生成相机的外参矩阵
At time 1903.172
- Translation: [0.020, 0.008, 0.003]
- Rotation: in Quaternion [-0.500, 0.502, -0.501, 0.497]in RPY (radian) [-1.575, -0.001, -1.577]in RPY (degree) [-90.231, -0.060, -90.330]
姿态四元数中w为0.497

将生成的外参替换掉data中的数据
请添加图片描述

修正相机外参

设置VINS的输出路径，需要是绝对路径
必须要自己设置！！！
请添加图片描述
output文件夹中会生成 extrinsic_parameter.csv 文件，使用文本编辑器打开，将生成的外参复制到yaml文件中。

显示track_image、设置特征点

在 realsense_stereo_imu_config.yaml 中设置（我这个是realsense_d435i，不同相机文件名不一定相同）

# 显示track_image
show_track 设为 1# 设置最大特征点
max_cnt: 150            # max feature number in feature tracking
# 设置特征点之间的距离
min_dist: 30            # min distance between two features

gazebo建图

PS：建议使用砖墙进行建图

保存路径：

PX4-Autopilot/Tools/sitl_gazebo/models

请添加图片描述

仅使用双目进行定位（无imu）

%YAML:1.0#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
# 禁掉imu
imu: 0         
num_of_cam: 2  # 修改ros话题输入
imu_topic: "/mavros/imu/data_raw"
image0_topic: "/camera/infra1/image_raw"
image1_topic: "/camera/infra2/image_raw"
output_path: "/home/feng/Vision/output/"cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480# 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-matrixrows: 4cols: 4dt: ddata: [ -5.7586305857286746e-03, -4.0463318787729019e-03,9.9997523237933461e-01, 2.0329267950355900e-02,-9.9998287214160420e-01, -1.0224590553211677e-03,-5.7628118925283633e-03, 7.9325209639615653e-03,1.0457519809151661e-03, -9.9999129084997906e-01,-4.0403746097850135e-03, 2.8559824645148020e-03, 0., 0., 0., 1. ]body_T_cam1: !!opencv-matrixrows: 4cols: 4dt: ddata: [ -1.0021770212322867e-03, 3.6313480322730518e-04,9.9999943188700535e-01, 1.5285779565991807e-02,-9.9999216342926500e-01, -3.8303422615924010e-03,-1.0007788055728661e-03, -5.2435791444330505e-02,3.8299766679101843e-03, -9.9999259827824449e-01,3.6697063849344680e-04, 8.6931302450199057e-03, 0., 0., 0., 1. ]#Multiple thread support
multiple_thread: 1#feature traker paprameters
# 设置特征点
max_cnt: 300            # max feature number in feature tracking
min_dist: 10            # 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)