initial commit

2022-08-05 08:23:25 +03:00
commit 5ecdc6abcf
387 changed files with 3010095 additions and 0 deletions
--- a/config/uzhfpv_outdoor/estimator_config.yaml
+++ b/config/uzhfpv_outdoor/estimator_config.yaml
@@ -0,0 +1,118 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
+use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
+use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
+
+use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
+max_cameras: 2
+
+calib_cam_extrinsics: false
+calib_cam_intrinsics: true
+calib_cam_timeoffset: true
+
+max_clones: 11
+max_slam: 50
+max_slam_in_update: 25
+max_msckf_in_update: 30
+dt_slam_delay: 2
+
+gravity_mag: 9.81
+
+feat_rep_msckf: "GLOBAL_3D"
+feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
+feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
+
+# zero velocity update parameters we can use
+# we support either IMU-based or disparity detection.
+try_zupt: true
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.5
+zupt_noise_multiplier: 20
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0
+init_imu_thresh: 0.0
+init_max_disparity: 1.5
+init_max_features: 25
+
+init_dyn_mle_opt_calib: false
+init_dyn_mle_max_iter: 50
+init_dyn_mle_max_time: 0.05
+init_dyn_mle_max_threads: 6
+init_dyn_num_pose: 6
+init_dyn_min_deg: 20.0
+
+init_dyn_inflation_ori: 10
+init_dyn_inflation_vel: 100
+init_dyn_inflation_bg: 10
+init_dyn_inflation_ba: 100
+init_dyn_min_rec_cond: 1e-20
+
+init_dyn_bias_g: [0.0, 0.0, 0.0]
+init_dyn_bias_a: [0.0, 0.0, 0.0]
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false
+record_timing_filepath: "/tmp/traj_timing.txt"
+
+save_total_state: false
+filepath_est: "/tmp/ov_estimate.txt"
+filepath_std: "/tmp/ov_estimate_std.txt"
+filepath_gt: "/tmp/ov_groundtruth.txt"
+
+# ==================================================================
+# ==================================================================
+
+# our front-end feature tracking parameters
+# we have a KLT and descriptor based (KLT is better implemented...)
+use_klt: true
+num_pts: 100
+fast_threshold: 65
+grid_x: 20
+grid_y: 20
+min_px_dist: 15
+knn_ratio: 0.70
+track_frequency: 31.0
+downsample_cameras: false
+multi_threading: true
+histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
+
+# aruco tag tracker for the system
+# DICT_6X6_1000 from https://chev.me/arucogen/
+use_aruco: false
+num_aruco: 1024
+downsize_aruco: true
+
+# ==================================================================
+# ==================================================================
+
+# camera noises and chi-squared threshold multipliers
+up_msckf_sigma_px: 2
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 2
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: true
+mask0: "../../ov_data/masks/uzhfpv_outdoor_mask0.png" #relative to current file
+mask1: "../../ov_data/masks/uzhfpv_outdoor_mask1.png" #relative to current file
+
+# imu and camera spacial-temporal
+# imu config should also have the correct noise values
+relative_config_imu: "kalibr_imu_chain.yaml"
+relative_config_imucam: "kalibr_imucam_chain.yaml"
+
+
+
+
--- a/config/uzhfpv_outdoor/kalibr_imu_chain.yaml
+++ b/config/uzhfpv_outdoor/kalibr_imu_chain.yaml
@@ -0,0 +1,20 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+imu0:
+  T_i_b:
+    - [1.0, 0.0, 0.0, 0.0]
+    - [0.0, 1.0, 0.0, 0.0]
+    - [0.0, 0.0, 1.0, 0.0]
+    - [0.0, 0.0, 0.0, 1.0]
+  accelerometer_noise_density: 2.0000e-3  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+  accelerometer_random_walk: 3.0000e-3    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+  gyroscope_noise_density: 1.6968e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+  gyroscope_random_walk: 1.9393e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+  #  accelerometer_noise_density: 0.1   # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+  #  accelerometer_random_walk: 0.002   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+  #  gyroscope_noise_density: 0.05      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+  #  gyroscope_random_walk: 4.0e-05     # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+  model: calibrated
+  rostopic: /snappy_imu
+  time_offset: 0.0
+  update_rate: 200.0
--- a/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml
+++ b/config/uzhfpv_outdoor/kalibr_imucam_chain.yaml
@@ -0,0 +1,37 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+cam0:
+  T_cam_imu:
+    - [-0.03179778293757218, -0.9994933985910031, -0.001359107523862424, 0.021115239798621798]
+    - [0.012827844120885779, 0.0009515801497960164, -0.9999172670328424, -0.0008992998316121829]
+    - [0.9994120008362244, -0.03181258663210035, 0.012791087377928778, -0.009491094814035777]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [1]
+  camera_model: pinhole
+  distortion_coeffs: [-0.005719912631104124, 0.004742449009601135, 0.0012060658036136048,
+                      -0.001580292679344826]
+  distortion_model: equidistant
+  intrinsics: [277.4786896484645, 277.42548548840034, 320.1052053576385, 242.10083077857894]
+  resolution: [640, 480]
+  rostopic: /snappy_cam/stereo_l
+  timeshift_cam_imu: -0.007999243205055177
+cam1:
+  T_cam_imu:
+    - [-0.011450159873389598, -0.9998746482793399, -0.010935335712288774, -0.05828448770624624]
+    - [0.009171247533644289, 0.010830579777447058, -0.9998992883087583, -0.0002362068202437068]
+    - [0.999892385238307, -0.01154929737910465, 0.009046086032012068, -0.00947464531803495]
+    - [0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    - [0.9997470623689986, 0.009836089265916417, 0.020225296846065624, -0.07919358086270675]
+    - [-0.00975774768296796, 0.9999445171722606, -0.0039684930755682956, 0.000831414953842084]
+    - [-0.020263209141547188, 0.0037701359508940783, 0.9997875716521978, 0.00044568632114983057]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.009025009906076716, 0.009967427035376123, -0.0029538969814842117,
+                      -0.0003503551771748748]
+  distortion_model: equidistant
+  intrinsics: [276.78679780974477, 276.79332134030807, 314.2862327340746, 236.51313088043128]
+  resolution: [640, 480]
+  rostopic: /snappy_cam/stereo_r
+  timeshift_cam_imu: -0.007983859928063504