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无人机速度进行限制

相关推荐
GUPAOAI1 天前
阅兵背后的科技:战场上的目标检测与无人机巡检
人工智能·科技·深度学习·目标检测·计算机视觉·ai·无人机
Coovally AI模型快速验证1 天前
轻量级注意力模型HOTSPOT-YOLO:无人机光伏热异常检测新SOTA,mAP高达90.8%
人工智能·学习·yolo·计算机视觉·目标跟踪·无人机
Coovally AI模型快速验证1 天前
突破闭集限制:3D-MOOD 实现开集单目 3D 检测新 SOTA
人工智能·yolo·计算机视觉·3d·目标跟踪·无人机
云卓SKYDROID1 天前
无人机GPS悬停模块技术解析
人工智能·目标跟踪·无人机·高科技·航线系统
爱吃烤鸡翅的酸菜鱼1 天前
Ubuntu环境下的 RabbitMQ 安装与配置详细教程
后端·ubuntu·rabbitmq·java-rabbitmq
音视频牛哥1 天前
低空经济的中国式进化:无人机与实时视频链路的未来五年
音视频·无人机·大牛直播sdk·城市数字孪生音视频方案·ai 视频感知低延迟音视频·无人机巡检音视频方案·低空经济音视频方案
电力程序小学童1 天前
【无人机三维路径规划】基于遗传算法GA结合粒子群算法PSO无人机复杂环境避障三维路径规划(含GA和PSO对比)研究
算法·无人机·ga·pso·改进粒子群·粒子群·gapso
小O的算法实验室1 天前
2025年COR IOTJ SCI2区,灾后通信无人机基站位置优化和移动充电无人机路径规划,深度解析+性能实测
无人机·论文复现·智能算法·智能算法改进
云卓SKYDROID1 天前
无人机散热模块技术要点分析
无人机·科普·高科技·云卓科技·散热系统
云卓SKYDROID1 天前
无人机传感器技术要点与难点解析
人工智能·数码相机·无人机·高科技·云卓科技·固件升级