-
Notifications
You must be signed in to change notification settings - Fork 4
/
UAV2_params.yaml
200 lines (158 loc) · 6.1 KB
/
UAV2_params.yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
###########################
# Initial state estimates #
###########################
# Calascio ruin UAV 1 starting pose
# p: [0.0996536944321, 0.650105327481, 0.485353333074] # [x,y,z]
# v: [0.013064551, -0.677197571, 0.510333198] # [x,y,z]
# q: [0.999476479566, -0.02852409305, 0.0150073484925, -0.00281467611065] # [w,x,y,z]
# INVERARAY PARALLEL
# p: [-11.1995244281, 3.87857023599, 4.9476594537] # [x,y,z]
# v: [-0.6996209, 0.016600004, -0.01753103] # [x,y,z]
# q: [0.999445154086, -0.0134727065945, 0.0289138608056, -0.00958429992068] # [w,x,y,z]
# INVERARAY AROUND
p: [-24.4015633876, -19.4547448302, 1.07439301689] # [x,y,z]
v: [-0.06452398, -0.03162339, -0.01064564] # [x,y,z]
q: [0.719182364837, 0.0123231604252, 0.0252719357365, 0.694252256092] # [w,x,y,z]
# Mars sol 180
# p: [-4.82780528, 0.21814379, 0.95765092]
# v: [-0.171817 , -0.02520245, 0.00789492]
# q: [0.648205551803, -0.00438199934281, 0.0090781326934, 0.76139867888]
# Initial IMU bias estimates
b_w: [0.0, 0.0, 0.0] # [x,y,z]
b_a: [0.0, 0.0, 0.0] # [x,y,z]
# Initial standard deviation estimates [x,y,z]
sigma_dp: [0.0, 0.0, 0.0] # [m]
sigma_dv: [0.05, 0.05, 0.05] # [m/s]
sigma_dtheta: [3.0, 3.0, 3.0] # [deg]
sigma_dbw: [6.0, 6.0, 6.0] # [deg/s]
sigma_dba: [0.3, 0.3, 0.3] # [m/s^2]
###############
# Calibration #
###############
# VI Simulator
cam1_fx: 0.605053191
cam1_fy: 0.947916667
cam1_cx: 0.5000
cam1_cy: 0.5000
cam1_s: 0.0
cam1_img_width: 752
cam1_img_height: 480
cam1_q_ic: [0.004059133072894, -0.928890463133302, -0.370065735566358, 0.014049352983967] # [w,x,y,z]
cam1_p_ic: [0.05914814, -0.04309647, 0.03085167] # [x,y,z]
cam1_time_offset: -0.000000001 # 0.0
# Feature noise (normalized coordinate standard deviation)
# 8.75/(cam_1fx)/cam1_img_width
# sdt(px)/norm_camera_coordinate_()
sigma_img: 0.011307696 # sqrt(0.00012922762474977756 * 5.0)
#######
# IMU #
#######
# ADIS16448 accels (Astec)
n_a: 0.004 # Accel noise spectral density [m/s^2/sqrt(Hz)]
n_ba: 0.006 # Accel bias random walk [m/s^3/sqrt(Hz)]
# ADIS16448 gyros (Astec)
n_w: 0.0003394 # Gyro noise spectral density [rad/s/sqrt(Hz)]
n_bw: 0.000038785 # Gyro bias random walk [rad/s^2/sqrt(Hz)]
#######
# LRF #
#######
# Noise (standard deviation in m)
sigma_range: 0.05
##############
# Sun Sensor #
##############
# Currently unused
q_sc: [1.0, 0.0, 0.0, 0.0] # [w,x,y,z]
w_s: [0.0, 0.0, 1.0] # [x,y,z]
###########
# Tracker #
###########
#0-based maximal pyramid level number; if set to 0, pyramids are not used (single level),
# if set to 1, two levels are used, and so on; if pyramids are passed to input then
# algorithm will use as many levels as pyramids have but no more than maxLevel.
max_level: 2
# The algorithm calculates the minimum eigen value of a 2x2 normal matrix of optical
# flow equations (this matrix is called a spatial gradient matrix in [25]), divided
# by number of pixels in a window; if this value is less than minEigThreshold, then
# a corresponding feature is filtered out and its flow is not processed, so it allows
# to remove bad points and get a performance boost.
min_eig_thr: 0.0
# Size of the search window at each pyramid level.
win_size_w: 31
win_size_h: 31
fast_detection_delta: 20
non_max_supp: True
block_half_length: 20
margin: 20
n_feat_min: 400
# RANSAC: 8 / LMEDS: 4 (see OpenCV doc)
outlier_method: 8
# Parameter used for RANSAC. It is the maximum distance from a point to an epipolar line in pixels,
# beyond which the point is considered an outlier and is not used for computing the final fundamental
# matrix. It can be set to something like 1-3, depending on the accuracy of the point localization,
# image resolution, and the image noise.
outlier_param1: 0.3
# Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of confidence
# (probability) that the estimated matrix is correct.
outlier_param2: 0.99
# 1 <=> no tiling
n_tiles_h: 3 # 3 # INV PARALLEL # 4 INV AROUND
n_tiles_w: 3 # 3 # INV PARALLEL # 4 INV AROUND
max_feat_per_tile: 10000
##############
# SLAM-MSCKF #
##############
# Number of poses in the sliding window
n_poses_max: 15
# Number of SLAM features
n_slam_features_max: 15
# Initial inverse depth of SLAM features [1/m]
# (default: 1 / (2 * d_min) [Montiel, 2006])
# rho_0: 0.25 # CIRCLE LOW START
# rho_0: 0.043751393601 # INVERARAY SPARSE
# rho_0: 0.0303825167012 # INVERARAY PARALLEL
# rho_0: 0.371576345786 # Mars Sol 180
rho_0: 0.0750159696959 # INVERARAY AROUND
# Initial standard deviation of SLAM inverse depth [1/m]
# (default: 1 / (4 * d_min) [Montiel, 2006])
# sigma_rho_0: 0.125 # CIRCLE LOW START
# sigma_rho_0: 0.021875697 # INVERARAY SPARSE
# sigma_rho_0: 0.015191258 # INVERARAY PARALLEL
# sigma_rho_0: 0.185788173 # Mars Sol 180
sigma_rho_0: 0.037507985 # INVERARAY AROUND
# Number of IEKF iterations (1 <=> EKF)
iekf_iter: 1
# Minimum baseline to trigger MSCKF measurement (pixels)
msckf_baseline: 30.0
# Minimum track length for a visual feature to be processed
min_track_length: 15
########################
# MULTI UAV PARAMETERS #
########################
pr_desc_ratio_thr: 0.75
pr_desc_min_distance: 50.0
desc_type: 0 # DESCRIPTOR_TYPE { ORB=0, SIFT, SURF };
sigma_landmark: 0.02 # meters
descriptor_scale_factor: 1.2
descriptor_pyramid: 8
descriptor_patch_size: 51
pr_score_thr: 0.65
ci_msckf_w: 0.015 # Covariance intersection weight of the other UAVs. if < 0 it's going to perform the optimization problem.
ci_slam_w: 0.05 # Covariance intersection weight of the other UAVs. if < 0 it's going to perform the optimization problem.
# BoW ORB vocabulary
# vocabulary_path: '/home/viciopoli/JPL/devel/x_ws/src/x/Vocabulary/orb_thermal_voc.fbow' # Thermal voc must be a fbow
vocabulary_path: '/home/viciopoli/JPL/devel/x_ws/src/x/Vocabulary/visual_voc_3_4_dbow3.yaml' #
# vocabulary_path: '/code/catkin_ws/src/x-image/packages/x/Vocabulary/orb_mur.fbow' # this must be a .tar.gz file
########
# Misc #
########
# Gravity vector in world frame [m/s^2]
g: [0.0, 0.0, -9.81] # [x,y,z]
# state buffer size
state_buffer_size: 250
# Timeout before 3D trajectory disappears in GUI (s)
traj_timeout_gui: 3
# flags to play rosbags
use_sim_time: True
# Initialize at startup
init_at_startup: True