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

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

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

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

设置相机内参

启动无人机仿真,查看ros话题rostopic list

查看相机内参(仿真相机的左右目参数是一样的)

powershell 复制代码
rostopic echo /camera/infra1/camera_info
powershell 复制代码
#	根据下面这个矩阵填写相机的内参
#    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 复制代码
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.0
   k2: 0.0
   p1: 0.0
   p2: 0.0
projection_parameters:
   fx: 319.9348449707031
   fy: 319.9348449707031
   cx: 320.0
   cy: 240.0

设置相机外参

设置VINS话题输入

rostopic list中无人机或相机的imu话题、相机左右目话题

相机外参(gazebo中的imu很飘,不建议使用双目 + IMU进行定位)

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

添加tf

查看相对坐标

这里使用的是无人机的imu,故而使用body、camera计算外参

显示偏移和四元数

powershell 复制代码
rosrun tf tf_echo body camera

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

注: 此处的四元数q = [x、y、z、w]

PS:可以直接找AI算,输入一下信息进行计算

powershell 复制代码
生成相机的外参矩阵
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,不同相机文件名不一定相同)

powershell 复制代码
# 显示track_image
show_track 设为 1

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

gazebo建图

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

  • 保存路径:
powershell 复制代码
PX4-Autopilot/Tools/sitl_gazebo/models

仅使用双目进行定位(无imu)

powershell 复制代码
%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-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -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-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -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)

QGC限制无人机飞行速度

  • MPC_XY_VEL_MAX
  • MPC_Z_VEL_MAX_DN
  • MPC_Z_VEL_MAX_UP

参考文档

gazebo中vins-fusion在仿真小车上的部署
gazebo仿真跑VINS-Fusion双目视觉惯性SLAM
QGC地面站对PX4无人机速度进行限制

相关推荐
物联网老王2 小时前
Ubuntu Linux Cursor 安装与使用一
linux·运维·ubuntu
fangeqin11 小时前
ubuntu源码安装python3.13遇到Could not build the ssl module!解决方法
linux·python·ubuntu·openssl
云卓SKYDROID11 小时前
无人机3控接力模式技术分析
无人机·通道·遥控器·高科技·云卓科技
风口上的吱吱鼠14 小时前
Armbian 25.5.1 Noble Gnome 开启远程桌面功能
服务器·ubuntu·armbian
笑衬人心。14 小时前
Ubuntu 22.04 + MySQL 8 无密码登录问题与 root 密码重置指南
linux·mysql·ubuntu
云卓SKYDROID14 小时前
无人机载重模块技术要点分析
人工智能·无人机·科普·高科技·云卓科技
森焱森14 小时前
无人机三轴稳定化控制(1)____飞机的稳定控制逻辑
c语言·单片机·算法·无人机
通信与导航14 小时前
无人机Ku相控阵卫星通信系统技术说明
无人机·信息与通信·射频工程·基带工程
阿木实验室14 小时前
Science Robotics发表 | 20m/s自主飞行+避开2.5mm电线的微型无人机!
无人机·机器人前沿
云卓SKYDROID14 小时前
无人机RTK技术要点与难点分析
人工智能·无人机·科普·高科技·云卓科技