diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
index 9fe259edc..5d002c04e 100644
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -14,7 +14,8 @@ jobs:
uses: actions/checkout@v2
- name: Installing Ceres Solver
run: |
- sudo apt-get update
+ sudo apt-get update -y
+ sudo apt install -y libunwind-dev
sudo apt-get install -y cmake libgoogle-glog-dev libgflags-dev libatlas-base-dev libeigen3-dev libsuitesparse-dev libceres-dev
# cd ..
# git clone https://ceres-solver.googlesource.com/ceres-solver
@@ -27,7 +28,7 @@ jobs:
# cd ../../
- name: Configure and Build
run: |
- sudo apt-get update
+ sudo apt-get update -y
sudo apt-get install -y libeigen3-dev libopencv-dev libboost-all-dev
mkdir build
cd build
diff --git a/ReadMe.md b/ReadMe.md
index 9993ff8a6..d1698022a 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -21,7 +21,10 @@ details on what the system supports.
## News / Events
-* **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259).
+
+* **April 15, 2023** - Minor update to v2.6.3 to support incremental feature triangulation of active features for downstream applications, faster zero-velocity update, small bug fixes, some example realsense configurations, and cached fast state prediction. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.6.3) for details.
+* **April 3, 2023** - We have released a monocular plane-aided VINS, termed [ov_plane](https://github.com/rpng/ov_plane), which leverages the OpenVINS project. Both now support the released [Indoor AR Table](https://github.com/rpng/ar_table_dataset) dataset.
+* **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259) and v2.6.2 [PR#264](https://github.com/rpng/open_vins/pull/264).
* **March 14, 2022** - Initial dynamic initialization open sourcing, asynchronous subscription to inertial readings and publishing of odometry, support for lower frequency feature tracking. See v2.6 [PR#232](https://github.com/rpng/open_vins/pull/232) for details.
* **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See
v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details.
@@ -92,6 +95,13 @@ details on what the system supports.
## Codebase Extensions
+* **[ov_plane](https://github.com/rpng/ov_plane)** - A real-time monocular visual-inertial odometry (VIO) system which leverages
+ environmental planes. At the core it presents an efficient robust monocular-based plane detection algorithm which does
+ not require additional sensing modalities such as a stereo, depth camera or neural network. The plane detection and tracking
+ algorithm enables real-time regularization of point features to environmental planes which are either maintained in the state
+ vector as long-lived planes, or marginalized for efficiency. Planar regularities are applied to both in-state SLAM and
+ out-of-state MSCKF point features, enabling long-term point-to-plane loop-closures due to the large spacial volume of planes.
+
* **[vicon2gt](https://github.com/rpng/vicon2gt)** - This utility was created to generate groundtruth trajectories using
a motion capture system (e.g. Vicon or OptiTrack) for use in evaluating visual-inertial estimation systems.
Specifically we calculate the inertial IMU state (full 15 dof) at camera frequency rate and generate a groundtruth
diff --git a/config/euroc_mav/estimator_config.yaml b/config/euroc_mav/estimator_config.yaml
index a6904c24f..094958363 100644
--- a/config/euroc_mav/estimator_config.yaml
+++ b/config/euroc_mav/estimator_config.yaml
@@ -30,9 +30,9 @@ feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
try_zupt: false
zupt_chi2_multipler: 0 # set to 0 for only disp-based
zupt_max_velocity: 0.1
-zupt_noise_multiplier: 50
-zupt_max_disparity: 1.5 # set to 0 for only imu-based
-zupt_only_at_beginning: true
+zupt_noise_multiplier: 10
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
@@ -86,7 +86,7 @@ min_px_dist: 10 # distance between features (features near each other provide le
knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz)
downsample_cameras: false # will downsample image in half if true
-multi_threading: true # if should enable opencv multi threading
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/config/kaist/estimator_config.yaml b/config/kaist/estimator_config.yaml
index 46372cb32..1a2b986e3 100644
--- a/config/kaist/estimator_config.yaml
+++ b/config/kaist/estimator_config.yaml
@@ -13,7 +13,7 @@ calib_cam_timeoffset: true # degenerate motion
calib_imu_intrinsics: false
calib_imu_g_sensitivity: false
-max_clones: 8
+max_clones: 11
max_slam: 50
max_slam_in_update: 25
max_msckf_in_update: 50
@@ -28,9 +28,9 @@ 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.25 # set to 0 for only disp-based
+zupt_chi2_multipler: 0.5 # set to 0 for only disp-based
zupt_max_velocity: 0.1
-zupt_noise_multiplier: 5
+zupt_noise_multiplier: 1
zupt_max_disparity: 0.4 # set to 0 for only imu-based
zupt_only_at_beginning: false
@@ -85,7 +85,7 @@ min_px_dist: 20
knn_ratio: 0.65
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
fi_min_dist: 0.25
diff --git a/config/kaist/kalibr_imucam_chain.yaml b/config/kaist/kalibr_imucam_chain.yaml
index 172591ee0..efea16a78 100644
--- a/config/kaist/kalibr_imucam_chain.yaml
+++ b/config/kaist/kalibr_imucam_chain.yaml
@@ -2,9 +2,9 @@
cam0:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- - [-0.00413,-0.01966,0.99980,1.73944]
- - [-0.99993,-0.01095,-0.00435,0.27803]
- - [0.01103,-0.99975,-0.01962,-0.08785]
+ - [-0.00681,-0.01532,0.99987,1.71239]
+ - [-0.99998,0.00033,-0.00680,0.24740]
+ - [-0.00023,-0.99988,-0.01532,-0.11589]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [1]
camera_model: pinhole
@@ -16,9 +16,9 @@ cam0:
cam1:
T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
- - [-0.00768,-0.01509,0.99986,1.73376]
- - [-0.99988,-0.01305,-0.00788,-0.19706]
- - [0.01317,-0.99980,-0.01499,-0.08271]
+ - [-0.01036,-0.01075,0.99990,1.70544]
+ - [-0.99994,-0.00178,-0.01038,-0.22770]
+ - [0.00189,-0.99994,-0.01073,-0.11611]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
@@ -27,3 +27,4 @@ cam1:
intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
resolution: [1280, 560]
rostopic: /stereo/right/image_raw
+
diff --git a/config/kaist_vio/estimator_config.yaml b/config/kaist_vio/estimator_config.yaml
index 7018aef00..7b96ad6d9 100644
--- a/config/kaist_vio/estimator_config.yaml
+++ b/config/kaist_vio/estimator_config.yaml
@@ -86,7 +86,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false # will downsample image in half if true
-multi_threading: true # if should enable opencv multi threading
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
fi_max_dist: 10.0
diff --git a/config/rpng_aruco/estimator_config.yaml b/config/rpng_aruco/estimator_config.yaml
index c53017c93..9a9be23d3 100644
--- a/config/rpng_aruco/estimator_config.yaml
+++ b/config/rpng_aruco/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 20
knn_ratio: 0.85
track_frequency: 21.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/config/rpng_ironsides/estimator_config.yaml b/config/rpng_ironsides/estimator_config.yaml
index 00a2a867d..039cfd4b1 100644
--- a/config/rpng_ironsides/estimator_config.yaml
+++ b/config/rpng_ironsides/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.65
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
fi_min_dist: 1.0
@@ -113,8 +113,8 @@ up_aruco_chi2_multipler: 1
# masks for our images
use_mask: true
-mask0: "../../ov_data/masks/ironsides0.png" #relative to current file
-mask1: "../../ov_data/masks/ironsides1.png" #relative to current file
+mask0: "mask_ironsides0.png" #relative to current file
+mask1: "mask_ironsides1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
diff --git a/ov_data/masks/ironsides0.png b/config/rpng_ironsides/mask_ironsides0.png
similarity index 100%
rename from ov_data/masks/ironsides0.png
rename to config/rpng_ironsides/mask_ironsides0.png
diff --git a/ov_data/masks/ironsides0.xcf b/config/rpng_ironsides/mask_ironsides0.xcf
similarity index 100%
rename from ov_data/masks/ironsides0.xcf
rename to config/rpng_ironsides/mask_ironsides0.xcf
diff --git a/ov_data/masks/ironsides1.png b/config/rpng_ironsides/mask_ironsides1.png
similarity index 100%
rename from ov_data/masks/ironsides1.png
rename to config/rpng_ironsides/mask_ironsides1.png
diff --git a/ov_data/masks/ironsides1.xcf b/config/rpng_ironsides/mask_ironsides1.xcf
similarity index 100%
rename from ov_data/masks/ironsides1.xcf
rename to config/rpng_ironsides/mask_ironsides1.xcf
diff --git a/config/rpng_plane/estimator_config.yaml b/config/rpng_plane/estimator_config.yaml
new file mode 100644
index 000000000..cb4c23484
--- /dev/null
+++ b/config/rpng_plane/estimator_config.yaml
@@ -0,0 +1,119 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+use_imuavg: true # if using 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 between pairs
+max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
+
+calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
+calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
+calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
+
+max_clones: 11 # how many clones in the sliding window
+max_slam: 25 # number of features in our state vector
+max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
+max_msckf_in_update: 20 # how many MSCKF features to use in the update
+dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...)
+
+gravity_mag: 9.8065 # magnitude of gravity in this location
+
+feat_rep_msckf: "GLOBAL_3D"
+feat_rep_slam: "GLOBAL_3D"
+feat_rep_aruco: "GLOBAL_3D"
+
+# zero velocity update parameters we can use
+# we support either IMU-based or disparity detection.
+try_zupt: false
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 50
+zupt_max_disparity: 1.5 # set to 0 for only imu-based
+zupt_only_at_beginning: true
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0 # how many seconds to collect initialization information
+init_imu_thresh: 0.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
+init_max_disparity: 4.0 # max disparity to consider the platform stationary (dependent on resolution)
+init_max_features: 75 # how many features to track during initialization (saves on computation)
+
+init_dyn_use: false # if dynamic initialization should be used
+init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
+init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
+init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
+init_dyn_mle_max_threads: 6 # how many threads the MLE should use
+init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
+init_dyn_min_deg: 10.0 # orientation change needed to try to init
+
+init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
+init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
+init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
+init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
+init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion
+
+init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
+init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false # if we want to record timing information of the method
+record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
+
+# if we want to save the simulation state and its diagional covariance
+# use this with rosrun ov_eval error_simulation
+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 # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 8 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 8 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 20 # distance between features (features near each other provide less information)
+knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
+track_frequency: 31.0 # frequency we will perform feature tracking at (in frames per second / hertz)
+downsample_cameras: false # will downsample image in half if true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
+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: 1
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 1
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: false
+
+# 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"
+
+
+
+
diff --git a/config/rpng_plane/kalibr_imu_chain.yaml b/config/rpng_plane/kalibr_imu_chain.yaml
new file mode 100644
index 000000000..a1d0a3c12
--- /dev/null
+++ b/config/rpng_plane/kalibr_imu_chain.yaml
@@ -0,0 +1,16 @@
+%YAML:1.0
+
+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: 0.00207649074
+ accelerometer_random_walk: 0.00041327852
+ gyroscope_noise_density: 0.00020544166
+ gyroscope_random_walk: 1.110622e-05
+ model: calibrated
+ rostopic: /d455/imu
+ time_offset: 0.0
+ update_rate: 400
diff --git a/config/rpng_plane/kalibr_imucam_chain.yaml b/config/rpng_plane/kalibr_imucam_chain.yaml
new file mode 100644
index 000000000..8dfff7fcd
--- /dev/null
+++ b/config/rpng_plane/kalibr_imucam_chain.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+
+
+cam0:
+ T_cam_imu:
+ - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095]
+ - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722]
+ - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: []
+ camera_model: pinhole
+ distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453]
+ distortion_model: radtan
+ intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077]
+ resolution: [848, 480]
+ rostopic: /d455/color/image_raw
+ timeshift_cam_imu: 0.002524377913673846
+
+
diff --git a/config/rpng_sim/estimator_config.yaml b/config/rpng_sim/estimator_config.yaml
index 959ce48d8..384a2f6f5 100644
--- a/config/rpng_sim/estimator_config.yaml
+++ b/config/rpng_sim/estimator_config.yaml
@@ -28,11 +28,11 @@ feat_rep_aruco: "GLOBAL_3D"
# zero velocity update parameters we can use
# we support either IMU-based or disparity detection.
try_zupt: false
-zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_chi2_multipler: 1 # set to 0 for only disp-based
zupt_max_velocity: 0.1
-zupt_noise_multiplier: 50
-zupt_max_disparity: 0.5 # set to 0 for only imu-based
-zupt_only_at_beginning: true
+zupt_noise_multiplier: 1
+zupt_max_disparity: 0 # set to 0 for only imu-based
+zupt_only_at_beginning: false
# ==================================================================
# ==================================================================
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 21.0
downsample_cameras: false
-multi_threading: false
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/config/rpng_sim/kalibr_imucam_chain.yaml b/config/rpng_sim/kalibr_imucam_chain.yaml
index 7012144b5..ec98602e4 100644
--- a/config/rpng_sim/kalibr_imucam_chain.yaml
+++ b/config/rpng_sim/kalibr_imucam_chain.yaml
@@ -25,4 +25,30 @@ cam1:
distortion_model: radtan
intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
resolution: [752, 480]
- rostopic: /cam1/image_raw
\ No newline at end of file
+ rostopic: /cam1/image_raw
+cam2:
+ T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+ - [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975]
+ - [0.999557249008, 0.0149672133247, 0.025715529948, 0.124676986768]
+ - [-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: [1]
+ camera_model: pinhole
+ distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
+ distortion_model: radtan
+ intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+ resolution: [752, 480]
+ rostopic: /cam2/image_raw
+cam3:
+ T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+ - [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556]
+ - [0.999598781151, 0.0130119051815, 0.0251588363115, 0.2253689425024]
+ - [-0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: [0]
+ camera_model: pinhole
+ distortion_coeffs: [-0.28368365,0.07451284,-0.00010473,-3.55590700e-05]
+ distortion_model: radtan
+ intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+ resolution: [752, 480]
+ rostopic: /cam3/image_raw
\ No newline at end of file
diff --git a/config/rs_d455/estimator_config.yaml b/config/rs_d455/estimator_config.yaml
new file mode 100644
index 000000000..053c8a0cf
--- /dev/null
+++ b/config/rs_d455/estimator_config.yaml
@@ -0,0 +1,119 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+use_imuavg: true # if using 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 between pairs
+max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
+
+calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
+calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
+calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
+
+max_clones: 11 # how many clones in the sliding window
+max_slam: 50 # number of features in our state vector
+max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
+max_msckf_in_update: 40 # how many MSCKF features to use in the update
+dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)
+
+gravity_mag: 9.81 # magnitude of gravity in this location
+
+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: false
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 10
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0 # how many seconds to collect initialization information
+init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
+init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution)
+init_max_features: 75 # how many features to track during initialization (saves on computation)
+
+init_dyn_use: false # if dynamic initialization should be used
+init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
+init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
+init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
+init_dyn_mle_max_threads: 6 # how many threads the MLE should use
+init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
+init_dyn_min_deg: 10.0 # orientation change needed to try to init
+
+init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
+init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
+init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
+init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
+init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion
+
+init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
+init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false # if we want to record timing information of the method
+record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
+
+# if we want to save the simulation state and its diagional covariance
+# use this with rosrun ov_eval error_simulation
+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 # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 15 # distance between features (features near each other provide less information)
+knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
+track_frequency: 31.0 # frequency we will perform feature tracking at (in frames per second / hertz)
+downsample_cameras: false # will downsample image in half if true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
+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: 1
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 1
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: false
+
+# 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"
+
+
+
+
diff --git a/config/rs_d455/kalibr_imu_chain.yaml b/config/rs_d455/kalibr_imu_chain.yaml
new file mode 100644
index 000000000..a8ea0cb15
--- /dev/null
+++ b/config/rs_d455/kalibr_imu_chain.yaml
@@ -0,0 +1,25 @@
+%YAML:1.0
+
+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]
+ # Values from allan plots
+ # https://github.com/rpng/ar_table_dataset
+ #accelerometer_noise_density: 0.0010382453726199955
+ #accelerometer_random_walk: 4.132785219723178e-05
+ #gyroscope_noise_density: 0.00010272083263292572
+ #gyroscope_random_walk: 1.1106223553679963e-06
+ # Inflated values (to account for unmodelled effects)
+ # - white noise multiplied by 2
+ # - bias random walk multiplied by 10
+ accelerometer_noise_density: 0.00207649074
+ accelerometer_random_walk: 0.00041327852
+ gyroscope_noise_density: 0.00020544166
+ gyroscope_random_walk: 0.00001110622
+ model: calibrated
+ rostopic: /d455/imu
+ time_offset: 0.0
+ update_rate: 400
diff --git a/config/rs_d455/kalibr_imucam_chain.yaml b/config/rs_d455/kalibr_imucam_chain.yaml
new file mode 100644
index 000000000..8dfff7fcd
--- /dev/null
+++ b/config/rs_d455/kalibr_imucam_chain.yaml
@@ -0,0 +1,19 @@
+%YAML:1.0
+
+
+cam0:
+ T_cam_imu:
+ - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095]
+ - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722]
+ - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: []
+ camera_model: pinhole
+ distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453]
+ distortion_model: radtan
+ intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077]
+ resolution: [848, 480]
+ rostopic: /d455/color/image_raw
+ timeshift_cam_imu: 0.002524377913673846
+
+
diff --git a/config/rs_d455/launch_d455.example b/config/rs_d455/launch_d455.example
new file mode 100644
index 000000000..513225717
--- /dev/null
+++ b/config/rs_d455/launch_d455.example
@@ -0,0 +1,110 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/config/rs_t265/estimator_config.yaml b/config/rs_t265/estimator_config.yaml
new file mode 100644
index 000000000..827e0c998
--- /dev/null
+++ b/config/rs_t265/estimator_config.yaml
@@ -0,0 +1,121 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+use_imuavg: true # if using 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 between pairs
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
+
+calib_cam_extrinsics: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
+calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
+calib_cam_timeoffset: true # if timeoffset between camera and IMU should be optimized
+
+max_clones: 11 # how many clones in the sliding window
+max_slam: 50 # number of features in our state vector
+max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
+max_msckf_in_update: 40 # how many MSCKF features to use in the update
+dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)
+
+gravity_mag: 9.81 # magnitude of gravity in this location
+
+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: false
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 10
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0 # how many seconds to collect initialization information
+init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
+init_max_disparity: 10.0 # max disparity to consider the platform stationary (dependent on resolution)
+init_max_features: 75 # how many features to track during initialization (saves on computation)
+
+init_dyn_use: false # if dynamic initialization should be used
+init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
+init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
+init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
+init_dyn_mle_max_threads: 6 # how many threads the MLE should use
+init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
+init_dyn_min_deg: 10.0 # orientation change needed to try to init
+
+init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
+init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
+init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
+init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
+init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion
+
+init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
+init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false # if we want to record timing information of the method
+record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
+
+# if we want to save the simulation state and its diagional covariance
+# use this with rosrun ov_eval error_simulation
+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 # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 15 # distance between features (features near each other provide less information)
+knn_ratio: 0.70 # descriptor knn threshold for the top two descriptor matches
+track_frequency: 31.0 # frequency we will perform feature tracking at (in frames per second / hertz)
+downsample_cameras: false # will downsample image in half if true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
+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: 1
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 1
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: true
+mask0: "mask_t265_cam0.png" #relative to current file
+mask1: "mask_t265_cam1.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"
+
+
+
+
diff --git a/config/rs_t265/kalibr_imu_chain.yaml b/config/rs_t265/kalibr_imu_chain.yaml
new file mode 100644
index 000000000..f553b6349
--- /dev/null
+++ b/config/rs_t265/kalibr_imu_chain.yaml
@@ -0,0 +1,27 @@
+%YAML:1.0
+
+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]
+ # Values from allan plots
+ # https://github.com/rpng/ar_table_dataset
+ #accelerometer_noise_density: 0.001464164197530967
+ #accelerometer_random_walk: 6.696534319519138e-05
+ #gyroscope_noise_density: 0.00010833365085022152
+ #gyroscope_random_walk: 1.1687657234840692e-06
+ # Inflated values (to account for unmodelled effects)
+ # - white noise multiplied by 5
+ # - bias random walk multiplied by 10
+ accelerometer_noise_density: 0.00732082098
+ accelerometer_random_walk: 0.0006696534319519138
+ gyroscope_noise_density: 0.00054166825
+ gyroscope_random_walk: 1.1687657234840692e-05
+ model: calibrated
+ rostopic: /t265/imu
+ time_offset: 0.0
+ update_rate: 200
+
+
diff --git a/config/rs_t265/kalibr_imucam_chain.yaml b/config/rs_t265/kalibr_imucam_chain.yaml
new file mode 100644
index 000000000..d5e7f0ac9
--- /dev/null
+++ b/config/rs_t265/kalibr_imucam_chain.yaml
@@ -0,0 +1,39 @@
+%YAML:1.0
+
+
+cam0:
+ T_cam_imu:
+ - [-0.9998761771972654, 0.002831174545748783, 0.015479493663382282, 0.009847737933630705]
+ - [-0.0029326606695474597, -0.999974330610803, -0.0065374016913607265, 0.004044777973851876]
+ - [0.015460587788970944, -0.006581988314211809, 0.9998588138607628, -0.015241042987496874]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: [1]
+ camera_model: pinhole
+ distortion_coeffs: [-0.003269003229949738, 0.05405258144204682, -0.05159409563898941, 0.010749180190267004]
+ distortion_model: equidistant
+ intrinsics: [282.019963259348, 280.7145153126385, 415.9558137753508, 396.6613771975339]
+ resolution: [848, 800]
+ rostopic: /t265/fisheye1/image_raw
+ timeshift_cam_imu: 0.005618603727452336
+
+cam1:
+ T_cam_imu:
+ - [-0.9999406689745719, 0.000724075187245526, 0.010868957898945571, -0.052814022762086264]
+ - [-0.0008072860689765681, -0.9999703864793142, -0.00765339490774561, 0.0040117609094788035]
+ - [0.010863094397485106, -0.007661715182273837, 0.9999116417466981, -0.015518593201262289]
+ - [0.0, 0.0, 0.0, 1.0]
+ T_cn_cnm1:
+ - [0.999987149466532, 0.0021373753273530643, -0.004597012998122464, -0.06274034262839549]
+ - [-0.002142375274127713, 0.9999971186631936, -0.0010830020732420377, -2.841394090416243e-05]
+ - [0.0045946849706689046, 0.0010928366830696733, 0.9999888472268076, -0.00032738772888446857]
+ - [0.0, 0.0, 0.0, 1.0]
+ cam_overlaps: [0]
+ camera_model: pinhole
+ distortion_coeffs: [-0.0028480208540388407, 0.05429895807264252, -0.0516469474707301, 0.010954064142332987]
+ distortion_model: equidistant
+ intrinsics: [281.49219856329046, 280.3289056204996, 414.09933409091184, 406.02198481912353]
+ resolution: [848, 800]
+ rostopic: /t265/fisheye2/image_raw
+ timeshift_cam_imu: 0.005583860066266795
+
+
diff --git a/config/rs_t265/launch_t265.example b/config/rs_t265/launch_t265.example
new file mode 100644
index 000000000..2459a23dc
--- /dev/null
+++ b/config/rs_t265/launch_t265.example
@@ -0,0 +1,71 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/config/rs_t265/mask_t265_cam0.png b/config/rs_t265/mask_t265_cam0.png
new file mode 100644
index 000000000..4f0ecfb7e
Binary files /dev/null and b/config/rs_t265/mask_t265_cam0.png differ
diff --git a/config/rs_t265/mask_t265_cam0.xcf b/config/rs_t265/mask_t265_cam0.xcf
new file mode 100644
index 000000000..e4b7165eb
Binary files /dev/null and b/config/rs_t265/mask_t265_cam0.xcf differ
diff --git a/config/rs_t265/mask_t265_cam1.png b/config/rs_t265/mask_t265_cam1.png
new file mode 100644
index 000000000..c4426efc0
Binary files /dev/null and b/config/rs_t265/mask_t265_cam1.png differ
diff --git a/config/rs_t265/mask_t265_cam1.xcf b/config/rs_t265/mask_t265_cam1.xcf
new file mode 100644
index 000000000..1e2f7d2c0
Binary files /dev/null and b/config/rs_t265/mask_t265_cam1.xcf differ
diff --git a/config/tum_vi/estimator_config.yaml b/config/tum_vi/estimator_config.yaml
index be1f5680a..20f0db1be 100644
--- a/config/tum_vi/estimator_config.yaml
+++ b/config/tum_vi/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.65
track_frequency: 21.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
@@ -106,8 +106,8 @@ up_aruco_chi2_multipler: 1
# masks for our images
use_mask: true
-mask0: "../../ov_data/masks/tumvi0.png" #relative to current file
-mask1: "../../ov_data/masks/tumvi1.png" #relative to current file
+mask0: "mask_tumvi0.png" #relative to current file
+mask1: "mask_tumvi1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
diff --git a/ov_data/masks/tumvi0.png b/config/tum_vi/mask_tumvi0.png
similarity index 100%
rename from ov_data/masks/tumvi0.png
rename to config/tum_vi/mask_tumvi0.png
diff --git a/ov_data/masks/tumvi0.xcf b/config/tum_vi/mask_tumvi0.xcf
similarity index 100%
rename from ov_data/masks/tumvi0.xcf
rename to config/tum_vi/mask_tumvi0.xcf
diff --git a/ov_data/masks/tumvi1.png b/config/tum_vi/mask_tumvi1.png
similarity index 100%
rename from ov_data/masks/tumvi1.png
rename to config/tum_vi/mask_tumvi1.png
diff --git a/ov_data/masks/tumvi1.xcf b/config/tum_vi/mask_tumvi1.xcf
similarity index 100%
rename from ov_data/masks/tumvi1.xcf
rename to config/tum_vi/mask_tumvi1.xcf
diff --git a/config/uzhfpv_indoor/estimator_config.yaml b/config/uzhfpv_indoor/estimator_config.yaml
index 472aa6f93..5f3ab1223 100644
--- a/config/uzhfpv_indoor/estimator_config.yaml
+++ b/config/uzhfpv_indoor/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/config/uzhfpv_indoor_45/estimator_config.yaml b/config/uzhfpv_indoor_45/estimator_config.yaml
index 472aa6f93..5f3ab1223 100644
--- a/config/uzhfpv_indoor_45/estimator_config.yaml
+++ b/config/uzhfpv_indoor_45/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/config/uzhfpv_outdoor/estimator_config.yaml b/config/uzhfpv_outdoor/estimator_config.yaml
index 187f7530e..79dbddfef 100644
--- a/config/uzhfpv_outdoor/estimator_config.yaml
+++ b/config/uzhfpv_outdoor/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
@@ -106,8 +106,8 @@ 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
+mask0: "mask_uzhfpv_outdoor_mask0.png" #relative to current file
+mask1: "mask_uzhfpv_outdoor_mask1.png" #relative to current file
# imu and camera spacial-temporal
# imu config should also have the correct noise values
diff --git a/ov_data/masks/uzhfpv_outdoor_mask0.png b/config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask0.png
similarity index 100%
rename from ov_data/masks/uzhfpv_outdoor_mask0.png
rename to config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask0.png
diff --git a/ov_data/masks/uzhfpv_outdoor_mask0.xcf b/config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask0.xcf
similarity index 100%
rename from ov_data/masks/uzhfpv_outdoor_mask0.xcf
rename to config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask0.xcf
diff --git a/ov_data/masks/uzhfpv_outdoor_mask1.png b/config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask1.png
similarity index 100%
rename from ov_data/masks/uzhfpv_outdoor_mask1.png
rename to config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask1.png
diff --git a/ov_data/masks/uzhfpv_outdoor_mask1.xcf b/config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask1.xcf
similarity index 100%
rename from ov_data/masks/uzhfpv_outdoor_mask1.xcf
rename to config/uzhfpv_outdoor/mask_uzhfpv_outdoor_mask1.xcf
diff --git a/config/uzhfpv_outdoor_45/estimator_config.yaml b/config/uzhfpv_outdoor_45/estimator_config.yaml
index 5fefbe1aa..a1b000412 100644
--- a/config/uzhfpv_outdoor_45/estimator_config.yaml
+++ b/config/uzhfpv_outdoor_45/estimator_config.yaml
@@ -84,7 +84,7 @@ min_px_dist: 15
knn_ratio: 0.70
track_frequency: 31.0
downsample_cameras: false
-multi_threading: true
+num_opencv_threads: 4 # -1: auto, 0-1: serial, >1: number of threads
histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
# aruco tag tracker for the system
diff --git a/docs/bib/extra.bib b/docs/bib/extra.bib
index 10c58dc87..2f0999276 100644
--- a/docs/bib/extra.bib
+++ b/docs/bib/extra.bib
@@ -1,323 +1,338 @@
@article{Burri2016IJRR,
- title = {The EuRoC micro aerial vehicle datasets},
- author = {Burri, Michael and Nikolic, Janosch and Gohl, Pascal and Schneider, Thomas and Rehder, Joern and Omari, Sammy and Achtelik, Markus W and Siegwart, Roland},
- year = 2016,
- journal = {The International Journal of Robotics Research},
- publisher = {SAGE Publications Sage UK: London, England},
- volume = 35,
- number = 10,
- pages = {1157--1163}
+ title = {The EuRoC micro aerial vehicle datasets},
+ author = {Burri, Michael and Nikolic, Janosch and Gohl, Pascal and Schneider, Thomas and Rehder, Joern and Omari, Sammy and Achtelik, Markus W and Siegwart, Roland},
+ year = 2016,
+ journal = {The International Journal of Robotics Research},
+ publisher = {SAGE Publications Sage UK: London, England},
+ volume = 35,
+ number = 10,
+ pages = {1157--1163}
}
@misc{ceres-solver,
- title = {Ceres Solver},
- author = {Sameer Agarwal and Keir Mierle and Others},
- url = {http://ceres-solver.org},
- howpublished = {\url{http://ceres-solver.org}}
+ title = {Ceres Solver},
+ author = {Sameer Agarwal and Keir Mierle and Others},
+ url = {http://ceres-solver.org},
+ howpublished = {\url{http://ceres-solver.org}}
+}
+@inproceedings{Chen2023ICRA,
+ title = {Monocular Visual-Inertial Odometry with Planar Regularities},
+ author = {Chuchu Chen and Patrick Geneva and Yuxiang Peng and Woosik Lee and Guoquan Huang},
+ year = 2023,
+ booktitle = {2023 International Conference on Robotics and Automation (ICRA)},
+ url = {https://pgeneva.com/downloads/papers/Chen2023ICRA.pdf}
+}
+@book{Chirikjian2011,
+ title = {Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications},
+ author = {Chirikjian, Gregory},
+ year = 2011,
+ publisher = {Springer Science \& Business Media},
+ volume = 2
}
@inproceedings{Davidson2009ENC,
- title = {Improved vehicle positioning in urban environment through integration of GPS and low-cost inertial sensors},
- author = {Davidson, Pavel and Hautam{\"a}ki, Jani and Collin, Jussi and Takala, Jarmo},
- year = 2009,
- booktitle = {Proceedings of the European Navigation Conference (ENC), Naples, Italy},
- pages = {3--6},
- url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf}
+ title = {Improved vehicle positioning in urban environment through integration of GPS and low-cost inertial sensors},
+ author = {Davidson, Pavel and Hautam{\"a}ki, Jani and Collin, Jussi and Takala, Jarmo},
+ year = 2009,
+ booktitle = {Proceedings of the European Navigation Conference (ENC), Naples, Italy},
+ pages = {3--6},
+ url = {http://www.tkt.cs.tut.fi/research/nappo_files/1_C2.pdf}
}
@inproceedings{Delmerico2018ICRA,
- title = {A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots},
- author = {Delmerico, Jeffrey and Scaramuzza, Davide},
- year = 2018,
- booktitle = {2018 IEEE International Conference on Robotics and Automation (ICRA)},
- pages = {2502--2509},
- url = {http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf},
- organization = {IEEE}
+ title = {A benchmark comparison of monocular visual-inertial odometry algorithms for flying robots},
+ author = {Delmerico, Jeffrey and Scaramuzza, Davide},
+ year = 2018,
+ booktitle = {2018 IEEE International Conference on Robotics and Automation (ICRA)},
+ pages = {2502--2509},
+ url = {http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf},
+ organization = {IEEE}
}
@inproceedings{Delmerico2019ICRA,
- title = {Are we ready for autonomous drone racing? the UZH-FPV drone racing dataset},
- author = {Delmerico, Jeffrey and Cieslewski, Titus and Rebecq, Henri and Faessler, Matthias and Scaramuzza, Davide},
- year = 2019,
- booktitle = {2019 International Conference on Robotics and Automation (ICRA)},
- pages = {6713--6719},
- url = {http://rpg.ifi.uzh.ch/docs/ICRA19_Delmerico.pdf},
- organization = {IEEE}
+ title = {Are we ready for autonomous drone racing? the UZH-FPV drone racing dataset},
+ author = {Delmerico, Jeffrey and Cieslewski, Titus and Rebecq, Henri and Faessler, Matthias and Scaramuzza, Davide},
+ year = 2019,
+ booktitle = {2019 International Conference on Robotics and Automation (ICRA)},
+ pages = {6713--6719},
+ url = {http://rpg.ifi.uzh.ch/docs/ICRA19_Delmerico.pdf},
+ organization = {IEEE}
}
@inproceedings{Dong2012IROS,
- title = {Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration},
- author = {Dong-Si, Tue-Cuong and Mourikis, Anastasios I},
- year = 2012,
- booktitle = {2012 IEEE/RSJ International Conference on Intelligent Robots and Systems},
- pages = {1064--1071},
- organization = {IEEE}
+ title = {Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration},
+ author = {Dong-Si, Tue-Cuong and Mourikis, Anastasios I},
+ year = 2012,
+ booktitle = {2012 IEEE/RSJ International Conference on Intelligent Robots and Systems},
+ pages = {1064--1071},
+ organization = {IEEE}
+}
+@techreport{Eckenhoff2018TR,
+ title = {Continuous Preintegration Theory for Visual-Inertial Navigation},
+ author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang},
+ year = 2018,
+ number = {RPNG-2018-CPI},
+ note = {Available: \url{http://udel.edu/~ghuang/papers/tr_cpi.pdf}},
+ institution = {University of Delaware}
}
@article{Eckenhoff2019IJRR,
- title = {Closed-form preintegration methods for graph-based visual-inertial navigation},
- author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang},
- year = 2019,
- journal = {International Journal of Robotics Research},
- volume = 38,
- number = 5,
- doi = {10.1177/0278364919835021},
- url = {https://doi.org/10.1177/0278364919835021}
+ title = {Closed-form preintegration methods for graph-based visual-inertial navigation},
+ author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang},
+ year = 2019,
+ journal = {International Journal of Robotics Research},
+ volume = 38,
+ number = 5,
+ doi = {10.1177/0278364919835021},
+ url = {https://doi.org/10.1177/0278364919835021}
}
@inproceedings{Furgale2013IROS,
- title = {Unified temporal and spatial calibration for multi-sensor systems},
- author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland},
- year = 2013,
- booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems},
- pages = {1280--1286},
- organization = {IEEE}
+ title = {Unified temporal and spatial calibration for multi-sensor systems},
+ author = {Furgale, Paul and Rehder, Joern and Siegwart, Roland},
+ year = 2013,
+ booktitle = {2013 IEEE/RSJ International Conference on Intelligent Robots and Systems},
+ pages = {1280--1286},
+ organization = {IEEE}
+}
+@techreport{Geneva2020TRVICON2GT,
+ title = {vicon2gt: Derivations and Analysis},
+ author = {Patrick Geneva and Guoquan Huang},
+ year = 2020,
+ number = {RPNG-2020-VICON2GT},
+ url = {http://udel.edu/~ghuang/papers/tr_vicon2gt.pdf},
+ institution = {University of Delaware}
+}
+@techreport{Hesch2012TR,
+ title = {Observability-constrained Vision-aided Inertial Navigation},
+ author = {Joel A. Hesch and Dimitrios G. Kottas and Sean L. Bowman and Stergios I. Roumeliotis},
+ year = 2012,
+ note = {Available: \url{https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.721.6118&rep=rep1&type=pdf}},
+ institution = {Dept. of Computer Science \& Engineering, University of Minnesota}
+}
+@article{Hesch2013TRO,
+ title = {Consistency analysis and improvement of vision-aided inertial navigation},
+ author = {Hesch, Joel A and Kottas, Dimitrios G and Bowman, Sean L and Roumeliotis, Stergios I},
+ year = 2013,
+ journal = {IEEE Transactions on Robotics},
+ publisher = {IEEE},
+ volume = 30,
+ number = 1,
+ pages = {158--176}
}
@article{Huang2010IJRR,
- title = {Observability-based rules for designing consistent EKF SLAM estimators},
- author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I},
- year = 2010,
- journal = {The International Journal of Robotics Research},
- publisher = {SAGE Publications Sage UK: London, England},
- volume = 29,
- number = 5,
- pages = {502--528},
- url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.228.5851&rep=rep1&type=pdf}
+ title = {Observability-based rules for designing consistent EKF SLAM estimators},
+ author = {Huang, Guoquan P and Mourikis, Anastasios I and Roumeliotis, Stergios I},
+ year = 2010,
+ journal = {The International Journal of Robotics Research},
+ publisher = {SAGE Publications Sage UK: London, England},
+ volume = 29,
+ number = 5,
+ pages = {502--528},
+ url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.228.5851&rep=rep1&type=pdf}
}
@article{Jeon2021RAL,
- title = {Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle},
- author = {Jeon, Jinwoo and Jung, Sungwook and Lee, Eungchang and Choi, Duckyu and Myung, Hyun},
- year = 2021,
- journal = {IEEE Robotics and Automation Letters},
- publisher = {IEEE},
- volume = 6,
- number = 3,
- pages = {5332--5339}
+ title = {Run your visual-inertial odometry on NVIDIA Jetson: Benchmark tests on a micro aerial vehicle},
+ author = {Jeon, Jinwoo and Jung, Sungwook and Lee, Eungchang and Choi, Duckyu and Myung, Hyun},
+ year = 2021,
+ journal = {IEEE Robotics and Automation Letters},
+ publisher = {IEEE},
+ volume = 6,
+ number = 3,
+ pages = {5332--5339}
}
@article{Jeong2019IJRR,
- title = {Complex urban dataset with multi-level sensors from highly diverse urban environments},
- author = {Jeong, Jinyong and Cho, Younggun and Shin, Young-Sik and Roh, Hyunchul and Kim, Ayoung},
- year = 2019,
- journal = {The International Journal of Robotics Research},
- publisher = {SAGE Publications Sage UK: London, England},
- volume = 38,
- number = 6,
- pages = {642--657}
+ title = {Complex urban dataset with multi-level sensors from highly diverse urban environments},
+ author = {Jeong, Jinyong and Cho, Younggun and Shin, Young-Sik and Roh, Hyunchul and Kim, Ayoung},
+ year = 2019,
+ journal = {The International Journal of Robotics Research},
+ publisher = {SAGE Publications Sage UK: London, England},
+ volume = 38,
+ number = 6,
+ pages = {642--657}
}
@book{Kay1993,
- title = {Fundamentals of statistical signal processing},
- author = {Kay, Steven M},
- year = 1993,
- publisher = {Prentice Hall PTR},
- url = {http://users.isr.ist.utl.pt/~pjcro/temp/Fundamentals\%20Of\%20Statistical\%20Signal\%20Processing\%2D\%2DEstimation\%20Theory-Kay.pdf}
+ title = {Fundamentals of statistical signal processing},
+ author = {Kay, Steven M},
+ year = 1993,
+ publisher = {Prentice Hall PTR},
+ url = {http://users.isr.ist.utl.pt/~pjcro/temp/Fundamentals\%20Of\%20Statistical\%20Signal\%20Processing\%2D\%2DEstimation\%20Theory-Kay.pdf}
}
@inproceedings{Lee2020IROS,
- title = {Visual-inertial-wheel odometry with online calibration},
- author = {Lee, Woosik and Eckenhoff, Kevin and Yang, Yulin and Geneva, Patrick and Huang, Guoquan},
- year = 2020,
- booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
- pages = {4559--4566},
- url = {http://udel.edu/~ghuang/papers/tr_wheel-vio.pdf},
- organization = {IEEE}
-}
-@article{Li2013IJRR,
- title = {High-precision, consistent EKF-based visual-inertial odometry},
- author = {Li, Mingyang and Mourikis, Anastasios I},
- year = 2013,
- journal = {The International Journal of Robotics Research},
- publisher = {SAGE Publications Sage UK: London, England},
- volume = 32,
- number = 6,
- pages = {690--711}
-}
-@phdthesis{Li2014THESIS,
- title = {Visual-inertial odometry on resource-constrained systems},
- author = {Li, Mingyang},
- year = 2014,
- url = {https://escholarship.org/uc/item/4nn0j264},
- school = {UC Riverside}
-}
-@book{Maybeck1982STOC,
- title = {Stochastic models, estimation, and control},
- author = {Maybeck, Peter S},
- year = 1982,
- publisher = {Academic press},
- volume = 3,
- url = {https://books.google.com/books?id=L_YVMUJKNQUC}
+ title = {Visual-inertial-wheel odometry with online calibration},
+ author = {Lee, Woosik and Eckenhoff, Kevin and Yang, Yulin and Geneva, Patrick and Huang, Guoquan},
+ year = 2020,
+ booktitle = {2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+ pages = {4559--4566},
+ url = {http://udel.edu/~ghuang/papers/tr_wheel-vio.pdf},
+ organization = {IEEE}
}
@techreport{Li2012TR,
- title = {Consistency of EKF-Based Visual-Inertial Odometry},
- author = {Mingyang Li and Anastasios I. Mourikis},
- year = 2012,
- note = {Available: \url{https://intra.ece.ucr.edu/~mourikis/tech_reports/VIO.pdf}},
- institution = {Dept. of Electrical Engineering, University of California, Riverside}
-}
-@inproceedings{Li2014ICRA,
- title = {High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation},
- author = {Mingyang Li and Hongsheng Yu and Xing Zheng and and Anastasios I. Mourikis},
- year = 2014,
- month = {May},
- booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
- pages = {409--416},
- url = {https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.696.3790&rep=rep1&type=pdf}
-}
-@article{Schneider2019Sensor,
- title = {Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation},
- author = {Schneider, Thomas and Li, Mingyang and Cadena, Cesar and Nieto, Juan and Siegwart, Roland},
- year = 2019,
- journal = {IEEE Sensors Journal},
- publisher = {IEEE},
- volume = 19,
- number = 10,
- pages = {3846--3860},
- url = {https://arxiv.org/abs/1901.07242}
-}
-@book{Chirikjian2011,
- title = {Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications},
- author = {Chirikjian, Gregory},
- year = 2011,
- publisher = {Springer Science \& Business Media},
- volume = 2
-}
-@techreport{Eckenhoff2018TR,
- title = {Continuous Preintegration Theory for Visual-Inertial Navigation},
- author = {Kevin Eckenhoff and Patrick Geneva and Guoquan Huang},
- year = 2018,
- number = {RPNG-2018-CPI},
- note = {Available: \url{http://udel.edu/~ghuang/papers/tr_cpi.pdf}},
- institution = {University of Delaware}
+ title = {Consistency of EKF-Based Visual-Inertial Odometry},
+ author = {Mingyang Li and Anastasios I. Mourikis},
+ year = 2012,
+ note = {Available: \url{https://intra.ece.ucr.edu/~mourikis/tech_reports/VIO.pdf}},
+ institution = {Dept. of Electrical Engineering, University of California, Riverside}
}
-@inproceedings{Yang2020ICRA,
- title = {Analytic Combined IMU Integration (ACI$^2$) for Visual-Inertial Navigation},
- author = {Yulin Yang and B. P. W. Babu and Chuchu Chen and Guoquan Huang and Liu Ren},
- year = 2020,
- booktitle = {Proc. of the IEEE International Conference on Robotics and Automation},
- address = {Paris, France}
-}
-@techreport{Yang2019TR_ACI,
- title = {Supplementary Material: Analytic Combined IMU Integrator (ACI$^2$) For Visual Inertial Navigation},
- author = {Yulin Yang and Chuchu Chen and Guoquan Huang},
- year = 2019,
- note = {Available: \url{http://udel.edu/~yuyang/downloads/suplementary\_2020ICRA.pdf}},
- institution = {RPNG, University of Delaware}
-}
-@techreport{Hesch2012TR,
- title = {Observability-constrained Vision-aided Inertial Navigation},
- author = {Joel A. Hesch and Dimitrios G. Kottas and Sean L. Bowman and Stergios I. Roumeliotis},
- year = 2012,
- note = {Available: \url{https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.721.6118&rep=rep1&type=pdf}},
- institution = {Dept. of Computer Science \& Engineering, University of Minnesota}
+@article{Li2013IJRR,
+ title = {High-precision, consistent EKF-based visual-inertial odometry},
+ author = {Li, Mingyang and Mourikis, Anastasios I},
+ year = 2013,
+ journal = {The International Journal of Robotics Research},
+ publisher = {SAGE Publications Sage UK: London, England},
+ volume = 32,
+ number = 6,
+ pages = {690--711}
}
-@article{Hesch2013TRO,
- title = {Consistency analysis and improvement of vision-aided inertial navigation},
- author = {Hesch, Joel A and Kottas, Dimitrios G and Bowman, Sean L and Roumeliotis, Stergios I},
- year = 2013,
- journal = {IEEE Transactions on Robotics},
- publisher = {IEEE},
- volume = 30,
- number = 1,
- pages = {158--176}
+@inproceedings{Li2014ICRA,
+ title = {High-fidelity sensor modeling and self-calibration in vision-aided inertial navigation},
+ author = {Mingyang Li and Hongsheng Yu and Xing Zheng and and Anastasios I. Mourikis},
+ year = 2014,
+ month = {May},
+ booktitle = {IEEE International Conference on Robotics and Automation (ICRA)},
+ pages = {409--416},
+ url = {https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.696.3790&rep=rep1&type=pdf}
}
-@article{Yang2020RSS,
- title = {Online imu intrinsic calibration: Is it necessary?},
- author = {Yang, Yulin and Geneva, Patrick and Zuo, Xingxing and Huang, Guoquan},
- year = 2020,
- journal = {Proc. of Robotics: Science and Systems (RSS), Corvallis, Or},
- url = {http://www.roboticsproceedings.org/rss16/p026.pdf}
+@phdthesis{Li2014THESIS,
+ title = {Visual-inertial odometry on resource-constrained systems},
+ author = {Li, Mingyang},
+ year = 2014,
+ url = {https://escholarship.org/uc/item/4nn0j264},
+ school = {UC Riverside}
}
-@article{Rehder2017Sensors,
- title = {Camera/IMU calibration revisited},
- author = {Rehder, Joern and Siegwart, Roland},
- year = 2017,
- journal = {IEEE Sensors Journal},
- publisher = {IEEE},
- volume = 17,
- number = 11,
- pages = {3257--3268}
+@book{Maybeck1982STOC,
+ title = {Stochastic models, estimation, and control},
+ author = {Maybeck, Peter S},
+ year = 1982,
+ publisher = {Academic press},
+ volume = 3,
+ url = {https://books.google.com/books?id=L_YVMUJKNQUC}
}
@inproceedings{Mourikis2007ICRA,
- title = {A multi-state constraint Kalman filter for vision-aided inertial navigation},
- author = {Mourikis, Anastasios I and Roumeliotis, Stergios I},
- year = 2007,
- booktitle = {Proceedings 2007 IEEE International Conference on Robotics and Automation},
- pages = {3565--3572},
- url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf},
- organization = {IEEE}
+ title = {A multi-state constraint Kalman filter for vision-aided inertial navigation},
+ author = {Mourikis, Anastasios I and Roumeliotis, Stergios I},
+ year = 2007,
+ booktitle = {Proceedings 2007 IEEE International Conference on Robotics and Automation},
+ pages = {3565--3572},
+ url = {http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.437.1085&rep=rep1&type=pdf},
+ organization = {IEEE}
}
@article{Mueggler2018TRO,
- title = {Continuous-Time Visual-Inertial Odometry for Event Cameras},
- author = {E. Mueggler and G. Gallego and H. Rebecq and D. Scaramuzza},
- year = 2018,
- journal = {IEEE Transactions on Robotics},
- pages = {1--16},
- url = {http://rpg.ifi.uzh.ch/docs/TRO18_Mueggler.pdf}
+ title = {Continuous-Time Visual-Inertial Odometry for Event Cameras},
+ author = {E. Mueggler and G. Gallego and H. Rebecq and D. Scaramuzza},
+ year = 2018,
+ journal = {IEEE Transactions on Robotics},
+ pages = {1--16},
+ url = {http://rpg.ifi.uzh.ch/docs/TRO18_Mueggler.pdf}
}
@article{Patron2015IJCV,
- title = {A spline-based trajectory representation for sensor fusion and rolling shutter cameras},
- author = {Patron-Perez, Alonso and Lovegrove, Steven and Sibley, Gabe},
- year = 2015,
- journal = {International Journal of Computer Vision},
- publisher = {Springer},
- volume = 113,
- number = 3,
- pages = {208--219}
+ title = {A spline-based trajectory representation for sensor fusion and rolling shutter cameras},
+ author = {Patron-Perez, Alonso and Lovegrove, Steven and Sibley, Gabe},
+ year = 2015,
+ journal = {International Journal of Computer Vision},
+ publisher = {Springer},
+ volume = 113,
+ number = 3,
+ pages = {208--219}
}
@article{Qin2018TRO,
- title = {Vins-mono: A robust and versatile monocular visual-inertial state estimator},
- author = {Qin, Tong and Li, Peiliang and Shen, Shaojie},
- year = 2018,
- journal = {IEEE Transactions on Robotics},
- publisher = {IEEE},
- volume = 34,
- number = 4,
- pages = {1004--1020},
- url = {https://arxiv.org/pdf/1708.03852.pdf}
+ title = {Vins-mono: A robust and versatile monocular visual-inertial state estimator},
+ author = {Qin, Tong and Li, Peiliang and Shen, Shaojie},
+ year = 2018,
+ journal = {IEEE Transactions on Robotics},
+ publisher = {IEEE},
+ volume = 34,
+ number = 4,
+ pages = {1004--1020},
+ url = {https://arxiv.org/pdf/1708.03852.pdf}
}
@article{Ramanandan2011TITS,
- title = {Inertial navigation aiding by stationary updates},
- author = {Ramanandan, Arvind and Chen, Anning and Farrell, Jay A},
- year = 2011,
- journal = {IEEE Transactions on Intelligent Transportation Systems},
- publisher = {IEEE},
- volume = 13,
- number = 1,
- pages = {235--248}
+ title = {Inertial navigation aiding by stationary updates},
+ author = {Ramanandan, Arvind and Chen, Anning and Farrell, Jay A},
+ year = 2011,
+ journal = {IEEE Transactions on Intelligent Transportation Systems},
+ publisher = {IEEE},
+ volume = 13,
+ number = 1,
+ pages = {235--248}
+}
+@article{Rehder2017Sensors,
+ title = {Camera/IMU calibration revisited},
+ author = {Rehder, Joern and Siegwart, Roland},
+ year = 2017,
+ journal = {IEEE Sensors Journal},
+ publisher = {IEEE},
+ volume = 17,
+ number = 11,
+ pages = {3257--3268}
+}
+@article{Schneider2019Sensor,
+ title = {Observability-aware self-calibration of visual and inertial sensors for ego-motion estimation},
+ author = {Schneider, Thomas and Li, Mingyang and Cadena, Cesar and Nieto, Juan and Siegwart, Roland},
+ year = 2019,
+ journal = {IEEE Sensors Journal},
+ publisher = {IEEE},
+ volume = 19,
+ number = 10,
+ pages = {3846--3860},
+ url = {https://arxiv.org/abs/1901.07242}
}
@inproceedings{Schubert2018IROS,
- title = {The TUM VI benchmark for evaluating visual-inertial odometry},
- author = {Schubert, David and Goll, Thore and Demmel, Nikolaus and Usenko, Vladyslav and St { \"u } ckler, J { \"o } rg and Cremers, Daniel},
- year = 2018,
- booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
- pages = {1680--1687},
- url = {https://arxiv.org/pdf/1804.06120.pdf},
- organization = {IEEE}
+ title = {The TUM VI benchmark for evaluating visual-inertial odometry},
+ author = {Schubert, David and Goll, Thore and Demmel, Nikolaus and Usenko, Vladyslav and St { \"u } ckler, J { \"o } rg and Cremers, Daniel},
+ year = 2018,
+ booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+ pages = {1680--1687},
+ url = {https://arxiv.org/pdf/1804.06120.pdf},
+ organization = {IEEE}
}
@article{Trawny2005TR,
- title = {Indirect Kalman filter for 3D attitude estimation},
- author = {Trawny, Nikolas and Roumeliotis, Stergios I},
- year = 2005,
- journal = {University of Minnesota, Dept. of Comp. Sci. \& Eng., Tech. Rep},
- volume = 2,
- pages = 2005,
- url = {http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf}
+ title = {Indirect Kalman filter for 3D attitude estimation},
+ author = {Trawny, Nikolas and Roumeliotis, Stergios I},
+ year = 2005,
+ journal = {University of Minnesota, Dept. of Comp. Sci. \& Eng., Tech. Rep},
+ volume = 2,
+ pages = 2005,
+ url = {http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf}
}
@inproceedings{Wagstaff2017IPIN,
- title = {Improving foot-mounted inertial navigation through real-time motion classification},
- author = {Wagstaff, Brandon and Peretroukhin, Valentin and Kelly, Jonathan},
- year = 2017,
- booktitle = {2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)},
- pages = {1--8},
- url = {https://arxiv.org/pdf/1707.01152.pdf},
- organization = {IEEE}
+ title = {Improving foot-mounted inertial navigation through real-time motion classification},
+ author = {Wagstaff, Brandon and Peretroukhin, Valentin and Kelly, Jonathan},
+ year = 2017,
+ booktitle = {2017 International Conference on Indoor Positioning and Indoor Navigation (IPIN)},
+ pages = {1--8},
+ url = {https://arxiv.org/pdf/1707.01152.pdf},
+ organization = {IEEE}
}
@inproceedings{Wu2017ICRA,
- title = {Vins on wheels},
- author = {Wu, Kejian J and Guo, Chao X and Georgiou, Georgios and Roumeliotis, Stergios I},
- year = 2017,
- booktitle = {2017 IEEE International Conference on Robotics and Automation (ICRA)},
- pages = {5155--5162},
- url = {http://mars.cs.umn.edu/papers/KejianWu_VINSonWheels.pdf},
- organization = {IEEE}
+ title = {Vins on wheels},
+ author = {Wu, Kejian J and Guo, Chao X and Georgiou, Georgios and Roumeliotis, Stergios I},
+ year = 2017,
+ booktitle = {2017 IEEE International Conference on Robotics and Automation (ICRA)},
+ pages = {5155--5162},
+ url = {http://mars.cs.umn.edu/papers/KejianWu_VINSonWheels.pdf},
+ organization = {IEEE}
+}
+@techreport{Yang2019TR_ACI,
+ title = {Supplementary Material: Analytic Combined IMU Integrator (ACI$^2$) For Visual Inertial Navigation},
+ author = {Yulin Yang and Chuchu Chen and Guoquan Huang},
+ year = 2019,
+ note = {Available: \url{http://udel.edu/~yuyang/downloads/suplementary\_2020ICRA.pdf}},
+ institution = {RPNG, University of Delaware}
+}
+@inproceedings{Yang2020ICRA,
+ title = {Analytic Combined IMU Integration (ACI$^2$) for Visual-Inertial Navigation},
+ author = {Yulin Yang and B. P. W. Babu and Chuchu Chen and Guoquan Huang and Liu Ren},
+ year = 2020,
+ booktitle = {Proc. of the IEEE International Conference on Robotics and Automation},
+ address = {Paris, France}
+}
+@article{Yang2020RSS,
+ title = {Online imu intrinsic calibration: Is it necessary?},
+ author = {Yang, Yulin and Geneva, Patrick and Zuo, Xingxing and Huang, Guoquan},
+ year = 2020,
+ journal = {Proc. of Robotics: Science and Systems (RSS), Corvallis, Or},
+ url = {http://www.roboticsproceedings.org/rss16/p026.pdf}
}
@inproceedings{Zhang2018IROS,
- title = {A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry},
- author = {Zhang, Zichao and Scaramuzza, Davide},
- year = 2018,
- booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
- pages = {7244--7251},
- url = {http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf},
- organization = {IEEE}
+ title = {A tutorial on quantitative trajectory evaluation for visual(-inertial) odometry},
+ author = {Zhang, Zichao and Scaramuzza, Davide},
+ year = 2018,
+ booktitle = {2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+ pages = {7244--7251},
+ url = {http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf},
+ organization = {IEEE}
}
diff --git a/docs/dev-docker.dox b/docs/dev-docker.dox
index 9bef78bfc..234f5d3a1 100644
--- a/docs/dev-docker.dox
+++ b/docs/dev-docker.dox
@@ -1,7 +1,7 @@
/**
-@page dev-docker Building with Docker
+@page dev-docker Installation Guide (Docker)
@tableofcontents
diff --git a/docs/eval-timing.dox b/docs/eval-timing.dox
index d301934c5..eae5d509c 100644
--- a/docs/eval-timing.dox
+++ b/docs/eval-timing.dox
@@ -74,6 +74,21 @@ mean_time = 0.0128 | std = 0.0035 | 99th = 0.0208 | max = 0.0403 (total)
+@subsection eval-ov-timing-hist Script "timing_histogram"
+Generates a histogram plot of *binned* execution times for a specific class.
+This allows for inspection of the distribution of times as compared to just the mean of it.
+The file should be comma separated format, with the first column being the timing, and the last column being the total time.
+The middle columns should describe how much each component takes (whose names are extracted from the header of the csv file).
+
+@code{.shell-session}
+rosrun ov_eval timing_histogram
+rosrun ov_eval timing_histogram timing_mono_ethV101.txt 75
+@endcode
+
+
+@image html eval/timing_hist.png width=90%
+
+
@subsection eval-ov-timing-compare Script "timing_comparison"
@@ -94,7 +109,7 @@ Example output:
[TIME]: loading data for timing_mono
[TIME]: loaded 2817 timestamps from file (7 categories)!!
mean_time = 0.0037 | std = 0.0011 | 99th = 0.0063 | max = 0.0254 (tracking)
-mean_time = 0.0004 | std = 0.0001 | 99th = 0.0006 | max = 0.0014 ( propagation)
+mean_time = 0.0004 | std = 0.0001 | 99th = 0.0006 | max = 0.0014 (propagation)
mean_time = 0.0032 | std = 0.0022 | 99th = 0.0083 | max = 0.0309 (msckf update)
mean_time = 0.0034 | std = 0.0013 | 99th = 0.0063 | max = 0.0099 (slam update)
mean_time = 0.0012 | std = 0.0017 | 99th = 0.0052 | max = 0.0141 (slam delayed)
@@ -105,7 +120,7 @@ mean_time = 0.0128 | std = 0.0035 | 99th = 0.0208 | max = 0.0403 (total)
[TIME]: loading data for timing_stereo
[TIME]: loaded 2817 timestamps from file (7 categories)!!
mean_time = 0.0077 | std = 0.0020 | 99th = 0.0124 | max = 0.0219 (tracking)
-mean_time = 0.0004 | std = 0.0001 | 99th = 0.0007 | max = 0.0023 ( propagation)
+mean_time = 0.0004 | std = 0.0001 | 99th = 0.0007 | max = 0.0023 (propagation)
mean_time = 0.0081 | std = 0.0047 | 99th = 0.0189 | max = 0.0900 (msckf update)
mean_time = 0.0063 | std = 0.0023 | 99th = 0.0117 | max = 0.0151 (slam update)
mean_time = 0.0020 | std = 0.0026 | 99th = 0.0079 | max = 0.0205 (slam delayed)
diff --git a/docs/getting-started.dox b/docs/getting-started.dox
index 0098ce60a..62c7ad78d 100644
--- a/docs/getting-started.dox
+++ b/docs/getting-started.dox
@@ -28,14 +28,11 @@ We encourage users to look at the specific documentation for a detailed view of
The ov_eval library has a bunch of evaluation methods and scripts that one can use to generate research results for publication.
-
-
-
-
@section getting-started-more Getting Started Guides
-- @subpage gs-installing --- Installation guide for OpenVINS and dependencies
-- @subpage dev-docker --- Installing with Docker instead of from source
+- @subpage gs-installing --- OpenVINS and dependencies within the ROS-based ecosystem
+- @subpage gs-installing-free --- How to leverage OpenVINS directly and install it as global library
+- @subpage dev-docker --- How to use OpenVINS within Docker and work with containers
- @subpage gs-tutorial --- Simple tutorial on getting OpenVINS running out of the box.
- @subpage gs-datasets --- Links to supported datasets and configuration files
- @subpage gs-calibration --- Guide to how to calibration your own visual-inertial sensors.
diff --git a/docs/gs-calibration.dox b/docs/gs-calibration.dox
index 364abdbd9..62395a312 100644
--- a/docs/gs-calibration.dox
+++ b/docs/gs-calibration.dox
@@ -32,28 +32,29 @@ The first task is to calibrate the camera intrinsic values such as the focal len
Our group often uses the [Kalibr](https://github.com/ethz-asl/kalibr/) @cite Furgale2013IROS calibration toolbox to perform both intrinsic and extrinsic offline calibrations, by proceeding the following steps:
1. Clone and build the [Kalibr](https://github.com/ethz-asl/kalibr/) toolbox
-2. Print out a calibration board to use (we normally use the [Aprilgrid 6x6 0.8x0.8 m (A0 page)](https://drive.google.com/file/d/0B0T1sizOvRsUdjFJem9mQXdiMTQ/edit?usp=sharing))
+2. Print out a calibration board to use (we normally use the [Aprilgrid 6x6 0.8x0.8 m (A0 page)](https://drive.google.com/file/d/1TCZJ1KPJrsj3ffCNnj001ege54jffc19/view))
3. Ensure that your sensor driver is publishing onto ROS with correct timestamps.
4. Sensor preparations
- - Limit motion blur by decreasing exposure time
- - Publish at low framerate to allow for larger variance in dataset (2-5hz)
+ - Limit motion blur by decreasing exposure time (can be achieved through high framerate)
- Ensure that your calibration board can be viewed in all areas of the image
- - Ensure that your sensor is in focus (can use their *kalibr\_camera\_focus* or just manually)
+ - Ensure that your sensor is in focus (can use their `kalibr_camera_focus` or just manually)
5. Record a ROS bag and ensure that the calibration board can be seen from different orientations, distances, and in each part of the image plane. You can either move the calibration board and keep the camera still or move the camera and keep the calibration board stationary.
6. Finally run the calibration
- - Use the kalibr\_calibrate\_cameras with your specified topic
+ - Use the `kalibr_calibrate_cameras` with your specified topic
+ - We recommend filtering the bag with their `--bag-freq 10.0` to reduce the processing time
- Depending on amount of distortion, use the *pinhole-equi* for fisheye, or if a low distortion then use the *pinhole-radtan*
- Depending on how many frames are in your dataset this can take on the upwards of a few hours.
-7. Inspect the final result, pay close attention to the final reprojection error graphs, with a good calibration having less than < 0.2-0.5 pixel reprojection errors.
+7. Inspect the final result, pay close attention to the final reprojection error graphs, with a good calibration having less than < 0.2-0.5 pixel reprojection errors. If you are planning on performing online calibration of the camera, then larger values might be acceptable (e.g. 1 pixel), but a more accurate offline calibration is always preferred.
+@image html kalibr_reprojection.png width=60%
+An example script [calibrate_camera_static.sh](https://github.com/rpng/ar_table_dataset/blob/9d556a789e2d01387e5ba2aeb2453269bc2c4001/calibrate_camera_static.sh), dataset, and configuration can be found in our group's [ar\_table\_dataset](https://github.com/rpng/ar_table_dataset/) repository.
@section gs-calib-imu-static IMU Intrinsic Calibration (Offline)
-The other imporatnt intrinsic calibration is to compute the inertial sensor intrinsic noise characteristics,
+The other important intrinsic calibration is to compute the inertial sensor intrinsic noise characteristics,
which are needed for the batch optimization to calibrate the camera to IMU transform and in any VINS estimator so that we can properly probabilistically fuse the images and inertial readings.
-Unfortunately, there is no mature open sourced toolbox to find these values, while one can try our [kalibr_allan](https://github.com/rpng/kalibr_allan) project, which is not optimized though.
Specifically we are estimating the following noise parameters:
Parameter | YAML element | Symbol | Units
@@ -63,27 +64,31 @@ Accelerometer "white noise" | `accelerometer_noise_density` | \f$\sigma_a\f$ | \
Gyroscope "random walk" | `gyroscope_random_walk` | \f$\sigma_{b_g}\f$ | \f$\frac{rad}{s^2}\frac{1}{\sqrt{Hz}}\f$
Accelerometer "random walk" | `accelerometer_random_walk` | \f$\sigma_{b_a}\f$ | \f$\frac{m}{s^3}\frac{1}{\sqrt{Hz}}\f$
-
-The standard way as explained in
+The standard way as explained in
[[IEEE Standard Specification Format Guide and Test Procedure for Single-Axis Interferometric Fiber Optic Gyros](https://ieeexplore.ieee.org/document/660628/) (page 71, section C)]
is that we can compute an [Allan variance](https://en.wikipedia.org/wiki/Allan_variance) plot of the sensor readings over different observation times (see below).
-
@image html allanvar_gyro.png width=75%
-
As shown in the above figure, if we compute the Allan variance we we can look at the value of a line at \f$\tau=1\f$ with a -1/2 slope fitted to the left side of the plot to get the white noise of the sensor.
Similarly, a line with 1/2 fitted to the right side can be evaluated at \f$\tau=3\f$ to get the random walk noise.
-We have a package that can do this in matlab, but actual verification and conversion into a C++ codebase has yet to be done.
-Please refer to our [[kalibr_allan](https://github.com/rpng/kalibr_allan)] github project for details on how to generate this plot for your sensor and calculate the values.
-Note that one may need to inflate the calculated values by 10-20 times to get usable sensor values.
+We recommend using the [allan_variance_ros](https://github.com/ori-drs/allan_variance_ros) project to recover these parameters experimentally.
+1. Clone and build [allan_variance_ros](https://github.com/ori-drs/allan_variance_ros)
+2. Ensure that your sensor driver is publishing onto ROS with correct timestamps.
+3. Collect a *stationary* dataset on the upwards of 20 hours
+4. Finally run the calibration
+ - Construct a [config file](https://github.com/ori-drs/allan_variance_ros/blob/2430a4498e8487a0b311296f033869560663ef7a/config/realsense_t265.yaml) with the IMU topic, sensor rate, and datset time
+ - Generate the allan variance data files with teh `allan_variance` command
+ - Finally process the command via their `analysis.py` script
+5. Typically these noise values should be inflated by maybe 10-20x their values to take into account unmodelled errors (one can test to see how different inflations perform)
+An example script [calibrate_imu.sh](https://github.com/rpng/ar_table_dataset/blob/9d556a789e2d01387e5ba2aeb2453269bc2c4001/calibrate_imu.sh), dataset, and configuration can be found in our group's [ar\_table\_dataset](https://github.com/rpng/ar_table_dataset/) repository.
-@section gs-calib-cam-dynamic Dynamic IMU-Camera Calibration (Offline)
+@section gs-calib-cam-dynamic Dynamic IMU-Camera Calibration (Offline)
After obtaining the intrinsic calibration of both the camera and IMU, we can now perform dynamic calibration of the transform between the two sensors.
For this we again leverage the [[Kalibr](https://github.com/ethz-asl/kalibr/) calibration toolbox].
@@ -92,23 +97,23 @@ One needs to have at least one translational motion along with two degrees of or
We recommend having as much change in orientation as possible in order to ensure convergence.
1. Clone and build the [Kalibr](https://github.com/ethz-asl/kalibr/) toolbox
-2. Print out a calibration board to use (we normally use the [Aprilgrid 6x6 0.8x0.8 m (A0 page)](https://drive.google.com/file/d/0B0T1sizOvRsUdjFJem9mQXdiMTQ/edit?usp=sharing))
-3. Ensure that your sensor driver is publishing onto ROS with correct timestamps.
+2. Print out a calibration board to use (we normally use the [Aprilgrid 6x6 0.8x0.8 m (A0 page)](https://drive.google.com/file/d/1TCZJ1KPJrsj3ffCNnj001ege54jffc19/view))
+3. Ensure that your sensor driver is publishing onto ROS with correct timestamps (inspect the IMU time dt plot carefully in the PDF report!)
4. Sensor preparations
- Limit motion blur by decreasing exposure time
- Publish at high-ish framerate (20-30hz)
- Publish your inertial reading at a reasonable rate (200-500hz)
5. Record a ROS bag and ensure that the calibration board can be seen from different orientations, distances, and mostly in the center of the image. You should move in *smooth* non-jerky motions with a trajectory that excites as many orientation and translational directions as possible at the same time. A 30-60 second dataset is normally enough to allow for calibration.
6. Finally run the calibration
- - Use the *kalibr\_calibrate\_imu\_camera*
+ - Use the `kalibr_calibrate_imu_camera`
- Input your static calibration file which will have the camera topics in it
- - You will need to make an [imu.yaml](https://drive.google.com/file/d/0B0T1sizOvRsUSk9ReDlid0VSY3M/edit?usp=sharing) file with your noise parameters.
+ - You will need to make an [imu.yaml](https://drive.google.com/file/d/1F9e9I1Xdm14nJw_E1j06HyinlBxRp3yu/view) file with your noise parameters.
- Depending on how many frames are in your dataset this can take on the upwards of half a day.
-7. Inspect the final result. You will want to make sure that the spline fitted to the inertial reading was properly fitted. Ensure that your estimated biases do not leave your 3-sigma bounds. If they do your trajectory was too dynamic, or your noise values are not good. Sanity check your final rotation and translation with hand-measured values.
-
-
+7. Inspect the final result. You will want to make sure that the spline fitted to the inertial reading was properly fitted. Your accelerometer and gyroscope errors are within their 3-sigma bounds (if not then your IMU noise or the dataset are incorrect). Ensure that your estimated biases do not leave your 3-sigma bounds. If they do leave then your trajectory was too dynamic, or your noise values are not good. Sanity check your final rotation and translation with hand-measured values.
+@image html kalibr_accel_err.png width=60%
+An example script [calibrate_camera_dynamic.sh](https://github.com/rpng/ar_table_dataset/blob/9d556a789e2d01387e5ba2aeb2453269bc2c4001/calibrate_camera_dynamic.sh), dataset, and configuration can be found in our group's [ar\_table\_dataset](https://github.com/rpng/ar_table_dataset/) repository.
diff --git a/docs/gs-datasets.dox b/docs/gs-datasets.dox
index 05bc0634d..64a046c67 100644
--- a/docs/gs-datasets.dox
+++ b/docs/gs-datasets.dox
@@ -24,7 +24,7 @@ Please take a look at the [run_ros_eth.sh](https://github.com/rpng/open_vins/blo
@par Groundtruth on V1_01_easy
We have found that the groundtruth on the V1_01_easy dataset is not accurate in its orientation estimate.
- We have recomputed this by optimizing the inertial and vicon readings in a graph to get the trajectory of the imu.
+ We have recomputed this by optimizing the inertial and vicon readings in a graph to get the trajectory of the imu (refer to our [vicon2gt](https://github.com/rpng/vicon2gt) @cite Geneva2020TRVICON2GT project).
You can find the output at this [link](https://drive.google.com/drive/folders/1d62Q_RQwHzKLcIdUlTeBmojr7j0UL4sM?usp=sharing) and is what we normally use to evaluate the error on this dataset.
@m_div{m-text-center}
@@ -55,6 +55,7 @@ Not all datasets have groundtruth available throughout the entire trajectory as
There are quite a few very challenging outdoor handheld datasets which are a challenging direction for research.
Note that we focus on the room datasets as full 6 dof pose collection is available over the total trajectory.
+@image html datasets/tumvi.png width=90%
@m_class{m-block m-warning}
@@ -62,6 +63,7 @@ Note that we focus on the room datasets as full 6 dof pose collection is availab
These datasets have very non-static starts, as they are handheld, and the standstill initialization has issues handling this.
Thus careful tuning of the imu initialization threshold is typically needed to ensure that the initialized orientation and the zero velocity assumption are valid.
Please take a look at the [run_ros_tumvi.sh](https://github.com/rpng/open_vins/blob/master/ov_msckf/scripts/run_ros_tumvi.sh) script for some reasonable default values (they might still need to be tuned).
+ One can enable dynamic initialization to avoid this problem via the `init_dyn_use` configuration value.
@m_div{m-text-center}
@@ -77,6 +79,28 @@ Note that we focus on the room datasets as full 6 dof pose collection is availab
+@section gs-data-rpng-ar-table RPNG AR Table Dataset
+
+The [Indoor AR Table Visual-Inertial Datasets](https://github.com/rpng/ar_table_dataset/) @cite Chen2023ICRA were collected to demonstrate the impact of estimating long-term planar surfaces within a visual-inertial estimator.
+An Intel Realsense D4553 with 30Hz RGB-D (depth was not used) and 400Hz BMI055 IMU along with 100Hz OptiTrack poses were recorded in 1-2 minute segments.
+The groundtruth was recovered using the [vicon2gt](https://github.com/rpng/vicon2gt) utility @cite Geneva2020TRVICON2GT.
+
+@image html datasets/ar_table.jpg width=90%
+
+@m_div{m-text-center}
+| Dataset Name | Length (m) | Dataset Link | Size (GB) | Groundtruth Traj. | Config |
+|:------:|:-------:|:-------:|:-----:|:-----:|:-----:|
+| table1 | 56 | [rosbag](https://drive.google.com/file/d/1qpjOQQYB8rvozTTbGBvuWjiat9MlDaMi/view?usp=sharing) | 4.77 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table2 | 44 | [rosbag](https://drive.google.com/file/d/1RBt8xYDsegtRiY5gKVyg9WPhecVvdXqn/view?usp=sharing) | 5.54 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table3 | 88 | [rosbag](https://drive.google.com/file/d/1_uLTYC3i2b1tx85OpGlqecxXgaO7BDT_/view?usp=sharing) | 13.19 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table4 | 91 | [rosbag](https://drive.google.com/file/d/1nYno_ttjGHwCQoxgV5SisVTc19Cb5O5c/view?usp=sharing) | 11.49 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table5 | 75 | [rosbag](https://drive.google.com/file/d/1CjkGfSCD_hWAidKsJ7edhuWQg7CyGa3U/view?usp=sharing) | 11.66 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table6 | 50 | [rosbag](https://drive.google.com/file/d/1FAGk7ZjRKDTkf-lhW3sb4EPj6SsUhyeQ/view?usp=sharing) | 5.26 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table7 | 63 | [rosbag](https://drive.google.com/file/d/1yWZGo6N0qlP_fHMIsr0jayRP3tXCO54U/view?usp=sharing) | 9.02 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+| table8 | 125 | [rosbag](https://drive.google.com/file/d/1pIz9owK4PvIkL_1t6eTZwGMrsLNpHBwa/view?usp=sharing) | 16.01 | [link](https://github.com/rpng/open_vins/tree/master/ov_data/rpng_plane) | [config](https://github.com/rpng/open_vins/blob/master/config/rpng_plane) |
+@m_enddiv
+
+
@section gs-data-rpng RPNG OpenVINS Dataset
@@ -98,6 +122,8 @@ Here are the specifics of the sensors that each dataset uses:
- Kalibr calibration file can be found [here](https://drive.google.com/file/d/1bhn0GrIYNEeAabQAbWoP8l_514cJ0KrZ/view?usp=sharing)
+@image html datasets/neighborhood.png width=90%
+
@m_class{m-block m-warning}
@par Monocular Camera
@@ -136,6 +162,8 @@ Each of these datasets is picked up in the beginning and then set down, we norma
Please take a look at the [run_ros_uzhfpv.sh](https://github.com/rpng/open_vins/blob/master/ov_msckf/scripts/run_ros_uzhfpv.sh) script for some reasonable default values (they might still need to be tuned).
+@image html datasets/uzhfpv.png width=90%
+
@m_class{m-block m-warning}
@par Dataset Groundtruthing
@@ -143,8 +171,6 @@ Please take a look at the [run_ros_uzhfpv.sh](https://github.com/rpng/open_vins/
This is due to inaccurate groundtruth orientation estimates which are explain in their [report](https://fpv.ifi.uzh.ch/wp-content/uploads/2020/11/Ground-Truth-Rotation-Issue-Report.pdf) on the issue.
The basic summary is that it is hard to get an accurate orientation information due to the point-based Leica measurements used to groundtruth.
-
-
@m_div{m-text-center}
| Dataset Name | Length (m) | Dataset Link | Groundtruth Traj. | Config |
|-------------:|--------|--------------|------------------|------------------|
@@ -171,6 +197,7 @@ The [KAIST urban dataset](https://sites.google.com/view/complex-urban-dataset) @
The dataset was collected in Korea with a vehicle equipped with stereo camera pair, 2d SICK LiDARs, 3d Velodyne LiDAR, Xsens IMU, fiber optic gyro (FoG), wheel encoders, and RKT GPS.
The camera is 10 Hz, while the Xsens IMU is 100 Hz sensing rate.
A groundtruth "baseline" trajectory is also provided which is the resulting output from fusion of the FoG, RKT GPS, and wheel encoders.
+We provide [processing scripts](https://github.com/rpng/open_vins/tree/master/ov_data/kaist/matlab) to generate the calibration and groundtruth from the dataset's formats.
@m_class{m-block m-warning}
@@ -215,7 +242,7 @@ Typically we process the datasets at 1.5x rate so we get a ~20 Hz image feed and
The [KAIST VIO dataset](https://github.com/url-kaist/kaistviodataset) @cite Jeon2021RAL is a dataset of a MAV in an indoor 3.15 x 3.60 x 2.50 meter environment which undergoes various trajectory motions.
The camera is intel realsense D435i 25 Hz, while the IMU is 100 Hz sensing rate from the pixelhawk 4 unit.
A groundtruth "baseline" trajectory is also provided from a OptiTrack Mocap system at 50 Hz, the bag files have the marker body frame to IMU frame already applied.
-This topic has been provided in ov_data for convinces sake.
+This topic has been provided in ov_data for convenience sake.
@m_div{m-text-center}
| Dataset Name | Length (km) | Dataset Link | Groundtruth Traj. | Example Launch |
diff --git a/docs/gs-installing-free.dox b/docs/gs-installing-free.dox
new file mode 100644
index 000000000..7d1a1937d
--- /dev/null
+++ b/docs/gs-installing-free.dox
@@ -0,0 +1,68 @@
+/**
+
+
+@page gs-installing-free Installation Guide (ROS Free)
+
+We do support ROS-free builds, but don't recommend using this interface as we have limited support for it.
+You will need to ensure you have installed OpenCV 3 or 4, Eigen3, and Ceres which are the only dependencies.
+Each of the submodules can be build and installed as a library onto a linux-based system.
+For Ubuntu linux-based system the system dependencies are:
+
+@code{.shell-session}
+sudo apt-get install libeigen3-dev libboost-all-dev libceres-dev
+@endcode
+
+Refer to @ref gs-install-opencv and @ref gs-install-ceres if a custom version of these are desired.
+We have tested building with OpenCV 3.2, 3.3, 3.4, 4.2, and 4.5.
+
+If ROS is found on the system, one can use command line options to ensure the ROS free version is compiled with: `cmake -DENABLE_ROS=OFF ..`.
+There are additional cmake options that users might be interested in.
+Configure these with `cmake -D=ON ..` options.
+
+- `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (ensure to disable this)
+- `ENABLE_ARUCO_TAGS` - (default ON) - Enable or disable aruco tag (disable if no contrib modules)
+- `BUILD_OV_EVAL` - (default ON) - Enable or disable building of ov_eval (needs to be built separately)
+- `DISABLE_MATPLOTLIB` - (default OFF) - Disable or enable matplotlib plot scripts in ov_eval
+
+We can build and install the ov_msckf library and binary files as follows:
+
+@code{.shell-session}
+cd ~/github/
+git clone https://github.com/rpng/open_vins/
+cd open_vins/ov_msckf/
+mkdir build && cd build
+cmake -DENABLE_ROS=OFF ..
+make -j4
+sudo make install
+@endcode
+
+If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions with proper buffers.
+The simulator binary `run_simulation` can give you and example on how to do this without the buffering logic.
+
+To now link to the built project from a different project, one can configure the following in their CMakeLists.txt:
+
+@code{.cmake}
+# headers: /usr/local/include/open_vins/
+# library: /usr/local/lib/libov_msckf_lib.so
+set(OPENVINS_INCLUDE_DIR /usr/local/include/open_vins/)
+set(OPENVINS_LIBRARIES ov_msckf_lib)
+@endcode
+
+Note that it is important to also link to the Eigen3, OpenCV, and Ceres dependencies of OpenVINS.
+Finally it can all come together and then be directly used as:
+
+@code{.cmake}
+target_include_directories(your_binary PUBLIC
+ ${CMAKE_CURRENT_SOURCE_DIR}
+ ${EIGEN3_INCLUDE_DIR}
+ ${BOOST_INCLUDE_DIR}
+ ${OPENVINS_INCLUDE_DIR}
+)
+target_link_libraries(your_binary PRIVATE
+ ${CERES_LIBRARIES}
+ ${OPENVINS_LIBRARIES}
+)
+@endcode
+
+
+*/
diff --git a/docs/gs-installing.dox b/docs/gs-installing.dox
index 7b006b123..f95290dab 100644
--- a/docs/gs-installing.dox
+++ b/docs/gs-installing.dox
@@ -1,7 +1,7 @@
/**
-@page gs-installing Installation Guide
+@page gs-installing Installation Guide (ROS1 and ROS2)
@tableofcontents
@@ -31,8 +31,9 @@ sudo apt-get install libeigen3-dev libboost-all-dev libceres-dev
If ROS is not found on the system, one can use command line options to run the simulation without any visualization or `cmake -DENABLE_ROS=OFF ..`.
-If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions.
+If you are using the ROS-free interface, you will need to properly construct the @ref ov_msckf::VioManagerOptions struct with proper information and feed inertial and image data into the correct functions with proper buffers.
The simulator binary `run_simulation` can give you and example on how to do this.
+To build without ROS installed, please refer to @ref gs-installing-free.
@@ -63,10 +64,10 @@ source ~/.bashrc
Otherwise, if you want to also install ROS2, you must *NOT* have a global source.
Instead we can have a nice helper command which can be used when we build a ROS1 workspace.
Additionally, the `source_devel` command can be used when in your workspace root to source built packages.
-Once appended simply run `source_ros1` to load your ROS1 environmental variables.
+Once appended simply run `ros1_source` to load your ROS1 environmental variables.
@code{.shell-session}
-echo "alias source_ros1=\"source /opt/ros/$ROS1_DISTRO/setup.bash\"" >> ~/.bashrc
+echo "alias ros1_source=\"source /opt/ros/$ROS1_DISTRO/setup.bash\"" >> ~/.bashrc
echo "alias source_devel=\"source devel/setup.bash\"" >> ~/.bashrc
source ~/.bashrc
@endcode
@@ -98,10 +99,10 @@ source ~/.bashrc
Otherwise, if you want to also install ROS1, you must *NOT* have a global source.
Instead we can have a nice helper command which can be used when we build a ROS1 workspace.
Additionally, the `source_install` command can be used when in your workspace root to source built packages.
-Once appended simply run `source_ros2` to load your ROS1 environmental variables.
+Once appended simply run `ros2_source` to load your ROS1 environmental variables.
@code{.shell-session}
-echo "alias source_ros2=\"source /opt/ros/$ROS2_DISTRO/setup.bash\"" >> ~/.bashrc
+echo "alias ros2_source=\"source /opt/ros/$ROS2_DISTRO/setup.bash\"" >> ~/.bashrc
echo "alias source_install=\"source install/setup.bash\"" >> ~/.bashrc
source ~/.bashrc
@endcode
@@ -117,7 +118,7 @@ After the build is successful please following the @ref gs-tutorial guide on get
There are additional options that users might be interested in.
Configure these with `catkin build -D=OFF` or `cmake -D=ON ..` in the ROS free case.
-- `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (if it is found)
+- `ENABLE_ROS` - (default ON) - Enable or disable building with ROS (see @ref gs-installing-free)
- `ENABLE_ARUCO_TAGS` - (default ON) - Enable or disable aruco tag (disable if no contrib modules)
- `BUILD_OV_EVAL` - (default ON) - Enable or disable building of ov_eval
- `DISABLE_MATPLOTLIB` - (default OFF) - Disable or enable matplotlib plot scripts in ov_eval
@@ -132,21 +133,9 @@ colcon build # ROS2
colcon build --event-handlers console_cohesion+ --packages-select ov_core ov_init ov_msckf ov_eval # ROS2 with verbose output
@endcode
-If you do not have ROS installed, then you can do the following:
-
-
-@code{.shell-session}
-cd ~/github/
-git clone https://github.com/rpng/open_vins/
-cd open_vins/ov_msckf/
-mkdir build
-cd build
-cmake ..
-make -j4
-@endcode
-
-If you wish to debug and run with assert statements, you can configure your workspace as follows:
-
+To build without ROS installed, please refer to @ref gs-installing-free.
+We recommend compiling with RelWithDebInfo as we always include [-g](https://gcc.gnu.org/onlinedocs/gcc/Debugging-Options.html) for debug symbols.
+If you wish to Debug, the workspace can configure as:
@code{.shell-session}
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Debug
@@ -183,6 +172,7 @@ One should make sure you can see some of the "contrib" (e.g. aruco) when you cma
@par OpenCV Source Installation
Try to first build with your system / ROS OpenCV.
+ We have tested building with OpenCV 3.2, 3.3, 3.4, 4.2, and 4.5.
Only fall back onto this if it does not allow you to compile, or want a newer version!
@code{.shell-session}
diff --git a/docs/img/allanvar_gyro.png b/docs/img/allanvar_gyro.png
index 7b365835b..4426a5021 100644
Binary files a/docs/img/allanvar_gyro.png and b/docs/img/allanvar_gyro.png differ
diff --git a/docs/img/datasets/ar_table.jpg b/docs/img/datasets/ar_table.jpg
new file mode 100644
index 000000000..5ae6a9ec8
Binary files /dev/null and b/docs/img/datasets/ar_table.jpg differ
diff --git a/docs/img/datasets/neighborhood.png b/docs/img/datasets/neighborhood.png
new file mode 100644
index 000000000..aa05e39e1
Binary files /dev/null and b/docs/img/datasets/neighborhood.png differ
diff --git a/docs/img/datasets/tumvi.png b/docs/img/datasets/tumvi.png
new file mode 100644
index 000000000..cbb8d17ea
Binary files /dev/null and b/docs/img/datasets/tumvi.png differ
diff --git a/docs/img/datasets/uzhfpv.png b/docs/img/datasets/uzhfpv.png
new file mode 100644
index 000000000..bae6f57f6
Binary files /dev/null and b/docs/img/datasets/uzhfpv.png differ
diff --git a/docs/img/eval/timing_hist.png b/docs/img/eval/timing_hist.png
new file mode 100644
index 000000000..3e8d3c836
Binary files /dev/null and b/docs/img/eval/timing_hist.png differ
diff --git a/docs/img/kalibr_accel_err.png b/docs/img/kalibr_accel_err.png
new file mode 100644
index 000000000..69d81a66f
Binary files /dev/null and b/docs/img/kalibr_accel_err.png differ
diff --git a/docs/img/kalibr_reprojection.png b/docs/img/kalibr_reprojection.png
new file mode 100644
index 000000000..36aea12fa
Binary files /dev/null and b/docs/img/kalibr_reprojection.png differ
diff --git a/docs/img/rig_hydra_old.jpg b/docs/img/rig_hydra_old.jpg
index 2bc0e8d07..096550c68 100644
Binary files a/docs/img/rig_hydra_old.jpg and b/docs/img/rig_hydra_old.jpg differ
diff --git a/docs/img/visensor.png b/docs/img/visensor.png
index d7243e24b..7806fe2a1 100644
Binary files a/docs/img/visensor.png and b/docs/img/visensor.png differ
diff --git a/ov_core/CMakeLists.txt b/ov_core/CMakeLists.txt
index 2308d6d01..a4a1bc23d 100644
--- a/ov_core/CMakeLists.txt
+++ b/ov_core/CMakeLists.txt
@@ -40,11 +40,6 @@ set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
-# Default build as release
-if (NOT CMAKE_BUILD_TYPE)
- set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE)
-endif ()
-
# Enable compile optimizations
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
diff --git a/ov_core/cmake/ROS1.cmake b/ov_core/cmake/ROS1.cmake
index 181dc3e7d..6db18fbeb 100644
--- a/ov_core/cmake/ROS1.cmake
+++ b/ov_core/cmake/ROS1.cmake
@@ -18,7 +18,7 @@ else ()
include(GNUInstallDirs)
set(CATKIN_PACKAGE_LIB_DESTINATION "${CMAKE_INSTALL_LIBDIR}")
set(CATKIN_PACKAGE_BIN_DESTINATION "${CMAKE_INSTALL_BINDIR}")
- set(CATKIN_GLOBAL_INCLUDE_DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}")
+ set(CATKIN_GLOBAL_INCLUDE_DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/open_vins/")
endif ()
# Include our header files
@@ -34,7 +34,7 @@ list(APPEND thirdparty_libraries
${Boost_LIBRARIES}
${OpenCV_LIBRARIES}
${catkin_LIBRARIES}
- )
+)
##################################################
# Make the core library
@@ -55,7 +55,7 @@ list(APPEND LIBRARY_SOURCES
src/feat/FeatureDatabase.cpp
src/feat/FeatureInitializer.cpp
src/utils/print.cpp
- )
+)
file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h")
add_library(ov_core_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS})
target_link_libraries(ov_core_lib ${thirdparty_libraries})
@@ -64,11 +64,11 @@ install(TARGETS ov_core_lib
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
+)
install(DIRECTORY src/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
- )
+)
##################################################
# Make binary files!
@@ -82,7 +82,7 @@ if (catkin_FOUND AND ENABLE_ROS)
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
+ )
endif ()
@@ -92,7 +92,15 @@ install(TARGETS test_webcam
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
- )
+)
+
+add_executable(test_profile src/test_profile.cpp)
+target_link_libraries(test_profile ov_core_lib ${thirdparty_libraries})
+install(TARGETS test_profile
+ ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+ RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
diff --git a/ov_core/cmake/ROS2.cmake b/ov_core/cmake/ROS2.cmake
index 43d7f82bd..b4ed9b7e3 100644
--- a/ov_core/cmake/ROS2.cmake
+++ b/ov_core/cmake/ROS2.cmake
@@ -23,7 +23,7 @@ include_directories(
list(APPEND thirdparty_libraries
${Boost_LIBRARIES}
${OpenCV_LIBRARIES}
- )
+)
##################################################
# Make the core library
@@ -44,7 +44,7 @@ list(APPEND LIBRARY_SOURCES
src/feat/FeatureDatabase.cpp
src/feat/FeatureInitializer.cpp
src/utils/print.cpp
- )
+)
file(GLOB_RECURSE LIBRARY_HEADERS "src/*.h")
add_library(ov_core_lib SHARED ${LIBRARY_SOURCES} ${LIBRARY_HEADERS})
ament_target_dependencies(ov_core_lib rclcpp cv_bridge)
@@ -54,11 +54,11 @@ install(TARGETS ov_core_lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
PUBLIC_HEADER DESTINATION include
- )
+)
install(DIRECTORY src/
DESTINATION include
FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp"
- )
+)
ament_export_include_directories(include)
ament_export_libraries(ov_core_lib)
@@ -77,5 +77,10 @@ ament_target_dependencies(test_webcam rclcpp cv_bridge)
target_link_libraries(test_webcam ov_core_lib ${thirdparty_libraries})
install(TARGETS test_webcam DESTINATION lib/${PROJECT_NAME})
+add_executable(test_profile src/test_profile.cpp)
+ament_target_dependencies(test_profile rclcpp cv_bridge)
+target_link_libraries(test_profile ov_core_lib ${thirdparty_libraries})
+install(TARGETS test_profile DESTINATION lib/${PROJECT_NAME})
+
# finally define this as the package
ament_package()
\ No newline at end of file
diff --git a/ov_core/package.xml b/ov_core/package.xml
index bd0b89b74..76192b66f 100644
--- a/ov_core/package.xml
+++ b/ov_core/package.xml
@@ -3,7 +3,7 @@
ov_core
- 2.6.0
+ 2.6.3
Core algorithms for visual-inertial navigation algorithms.
diff --git a/ov_core/src/cam/CamBase.h b/ov_core/src/cam/CamBase.h
index c8de21798..60788f903 100644
--- a/ov_core/src/cam/CamBase.h
+++ b/ov_core/src/cam/CamBase.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/cam/CamEqui.h b/ov_core/src/cam/CamEqui.h
index 0709d0f73..df727172d 100644
--- a/ov_core/src/cam/CamEqui.h
+++ b/ov_core/src/cam/CamEqui.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/cam/CamRadtan.h b/ov_core/src/cam/CamRadtan.h
index 4f8be1564..80e0c5bae 100644
--- a/ov_core/src/cam/CamRadtan.h
+++ b/ov_core/src/cam/CamRadtan.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/dummy.cpp b/ov_core/src/dummy.cpp
index a20501b9f..52d76b678 100644
--- a/ov_core/src/dummy.cpp
+++ b/ov_core/src/dummy.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/Feature.cpp b/ov_core/src/feat/Feature.cpp
index e82b174af..05532d102 100644
--- a/ov_core/src/feat/Feature.cpp
+++ b/ov_core/src/feat/Feature.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/Feature.h b/ov_core/src/feat/Feature.h
index 9983847d0..f0bd8a8c0 100644
--- a/ov_core/src/feat/Feature.h
+++ b/ov_core/src/feat/Feature.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/FeatureDatabase.cpp b/ov_core/src/feat/FeatureDatabase.cpp
index 5cb8de5ce..0e18b248e 100644
--- a/ov_core/src/feat/FeatureDatabase.cpp
+++ b/ov_core/src/feat/FeatureDatabase.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -267,7 +267,7 @@ double FeatureDatabase::get_oldest_timestamp() {
double oldest_time = -1;
for (auto const &feat : features_idlookup) {
for (auto const &camtimepair : feat.second->timestamps) {
- if (!camtimepair.second.empty() && (oldest_time == -1 || oldest_time < camtimepair.second.at(0))) {
+ if (!camtimepair.second.empty() && (oldest_time == -1 || oldest_time > camtimepair.second.at(0))) {
oldest_time = camtimepair.second.at(0);
}
}
diff --git a/ov_core/src/feat/FeatureDatabase.h b/ov_core/src/feat/FeatureDatabase.h
index 7eae4de5c..df486227f 100644
--- a/ov_core/src/feat/FeatureDatabase.h
+++ b/ov_core/src/feat/FeatureDatabase.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/FeatureHelper.h b/ov_core/src/feat/FeatureHelper.h
index 6e08aa0d9..db35c555f 100644
--- a/ov_core/src/feat/FeatureHelper.h
+++ b/ov_core/src/feat/FeatureHelper.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -121,7 +121,7 @@ class FeatureHelper {
* @param oldest_time Only compute disparity for ones newer (-1 to disable)
*/
static void compute_disparity(std::shared_ptr db, double &disp_mean, double &disp_var, int &total_feats,
- double newest_time = 1, double oldest_time = 1) {
+ double newest_time = -1, double oldest_time = -1) {
// Compute the disparity
std::vector disparities;
diff --git a/ov_core/src/feat/FeatureInitializer.cpp b/ov_core/src/feat/FeatureInitializer.cpp
index a30c1405e..05b8d9e1b 100644
--- a/ov_core/src/feat/FeatureInitializer.cpp
+++ b/ov_core/src/feat/FeatureInitializer.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/FeatureInitializer.h b/ov_core/src/feat/FeatureInitializer.h
index 394b690f8..6477f6b81 100644
--- a/ov_core/src/feat/FeatureInitializer.h
+++ b/ov_core/src/feat/FeatureInitializer.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/feat/FeatureInitializerOptions.h b/ov_core/src/feat/FeatureInitializerOptions.h
index 2669c03a1..52a84c442 100644
--- a/ov_core/src/feat/FeatureInitializerOptions.h
+++ b/ov_core/src/feat/FeatureInitializerOptions.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/plot/matplotlibcpp.h b/ov_core/src/plot/matplotlibcpp.h
index b49827a31..dd8be4e2d 100644
--- a/ov_core/src/plot/matplotlibcpp.h
+++ b/ov_core/src/plot/matplotlibcpp.h
@@ -1398,9 +1398,9 @@ template inline void yticks(const std::vector &ticks
inline void subplot(long nrows, long ncols, long plot_number) {
// construct positional args
PyObject *args = PyTuple_New(3);
- PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows));
- PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols));
- PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number));
+ PyTuple_SetItem(args, 0, PyLong_FromLong(nrows));
+ PyTuple_SetItem(args, 1, PyLong_FromLong(ncols));
+ PyTuple_SetItem(args, 2, PyLong_FromLong(plot_number));
PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args);
if (!res)
diff --git a/ov_core/src/sim/BsplineSE3.cpp b/ov_core/src/sim/BsplineSE3.cpp
index 721f28ea4..48a73c54a 100644
--- a/ov_core/src/sim/BsplineSE3.cpp
+++ b/ov_core/src/sim/BsplineSE3.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -51,7 +51,7 @@ void BsplineSE3::feed_trajectory(std::vector traj_points) {
if (pose.first <= timestamp_min) {
timestamp_min = pose.first;
}
- if (pose.first >= timestamp_min) {
+ if (pose.first >= timestamp_max) {
timestamp_max = pose.first;
}
}
diff --git a/ov_core/src/sim/BsplineSE3.h b/ov_core/src/sim/BsplineSE3.h
index e216750f0..205288f91 100644
--- a/ov_core/src/sim/BsplineSE3.h
+++ b/ov_core/src/sim/BsplineSE3.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/test_profile.cpp b/ov_core/src/test_profile.cpp
new file mode 100644
index 000000000..1dd4d91fd
--- /dev/null
+++ b/ov_core/src/test_profile.cpp
@@ -0,0 +1,332 @@
+/*
+ * OpenVINS: An Open Platform for Visual-Inertial Research
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
+ * Copyright (C) 2018-2019 Kevin Eckenhoff
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+
+#include
+
+#include "utils/print.h"
+
+// Define the function to be called when ctrl-c (SIGINT) is sent to process
+void signal_callback_handler(int signum) { std::exit(signum); }
+
+void print_stats(std::string title, std::vector times, std::string title2 = "", std::vector stats = {}) {
+
+ // Compute mean and rmse
+ double mean = 0.0;
+ double rmse = 0.0;
+ for (size_t i = 0; i < times.size(); i++) {
+ assert(!std::isnan(times.at(i)));
+ mean += times.at(i);
+ rmse += times.at(i) * times.at(i);
+ }
+ mean /= times.size();
+ rmse = std::sqrt(rmse / times.size());
+
+ // Using mean, compute standard deviation
+ double std = 0;
+ for (size_t i = 0; i < times.size(); i++) {
+ std += std::pow(times.at(i) - mean, 2);
+ }
+ std = std::sqrt(std / (times.size() - 1));
+
+ // Print!
+ if (stats.empty()) {
+ PRINT_INFO("%s: %.3f +- %.3fms\n", title.c_str(), mean, std);
+ } else {
+ double mean2 = 0.0;
+ for (size_t i = 0; i < stats.size(); i++) {
+ assert(!std::isnan(stats.at(i)));
+ mean2 += stats.at(i);
+ }
+ mean2 /= stats.size();
+ PRINT_INFO("%s: %.3f +- %.3f ms (%.2f avg %s)\n", title.c_str(), mean, std, mean2, title2.c_str());
+ }
+}
+
+// Main function
+int main(int argc, char **argv) {
+
+ // Verbosity
+ std::string verbosity = "INFO";
+ ov_core::Printer::setPrintLevel(verbosity);
+
+ // Parameters used in all algorithms
+ int num_trials = 100;
+ int big_matrix = 1000;
+ int big_matrix_eigen1 = 300;
+ int big_matrix_eigen2 = 900;
+ int pyr_levels = 5;
+ int fast_threshold = 30;
+ int max_features = 500;
+ cv::Size win_size = cv::Size(15, 15);
+ cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01);
+
+ // Helper data structures
+ cv::Mat img1(big_matrix, big_matrix, CV_8UC1);
+ cv::Mat img3(big_matrix, big_matrix, CV_8UC3);
+ std::vector times_ms;
+ std::vector extra_stats;
+
+ // OPENCV: RANDOM BIG IMAGE
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ cv::randu(img3, cv::Scalar(0, 0, 0), cv::Scalar(255, 255, 255));
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("OPENCV: RANDOM BIG IMAGE", times_ms);
+
+ // OPENCV: HISTOGRAM EQUALIZATION
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ cv::randu(img1, cv::Scalar(0), cv::Scalar(255));
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ cv::equalizeHist(img1, img1);
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("OPENCV: HISTOGRAM EQUALIZATION", times_ms);
+
+ // OPENCV: BUILD OPTICAL FLOW PYRAMID
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ cv::randu(img1, cv::Scalar(0), cv::Scalar(255));
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ std::vector imgpyr;
+ cv::buildOpticalFlowPyramid(img1, imgpyr, win_size, pyr_levels);
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("OPENCV: BUILD OPTICAL FLOW PYRAMID", times_ms);
+
+ // OPENCV: FAST FEATURE EXTRACTION
+ times_ms.clear();
+ extra_stats.clear();
+ for (int i = 0; i < num_trials; i++) {
+ cv::randu(img1, cv::Scalar(0), cv::Scalar(255));
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ std::vector pts_new;
+ cv::FAST(img1, pts_new, fast_threshold, true);
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ extra_stats.push_back((int)pts_new.size());
+ }
+ print_stats("OPENCV: FAST FEATURE EXTRACTION", times_ms, "feats", extra_stats);
+
+ // OPENCV: KLT OPTICAL FLOW
+ times_ms.clear();
+ extra_stats.clear();
+ for (int i = 0; i < num_trials; i++) {
+ std::vector imgpyr1, imgpyr2;
+ cv::buildOpticalFlowPyramid(img1, imgpyr1, win_size, pyr_levels);
+ std::vector pts0_tmp;
+ cv::FAST(img1, pts0_tmp, fast_threshold, true);
+ std::sort(pts0_tmp.begin(), pts0_tmp.end(), [](cv::KeyPoint first, cv::KeyPoint second) { return first.response > second.response; });
+ std::vector pts0;
+ for (size_t j = 0; j < pts0_tmp.size() && (int)j < max_features; j++)
+ pts0.push_back(pts0_tmp.at(j).pt);
+ cv::randu(img1, cv::Scalar(0), cv::Scalar(255)); // second image
+ cv::buildOpticalFlowPyramid(img1, imgpyr2, win_size, pyr_levels);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ std::vector mask_klt;
+ std::vector error;
+ std::vector pts1, pts1_good;
+ cv::calcOpticalFlowPyrLK(imgpyr1, imgpyr2, pts0, pts1, mask_klt, error, win_size, pyr_levels, term_crit);
+ for (size_t j = 0; j < mask_klt.size(); j++) {
+ if (mask_klt.at(j))
+ pts1_good.push_back(pts1.at(j));
+ }
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ extra_stats.push_back((int)pts1_good.size());
+ }
+ print_stats("OPENCV: KLT OPTICAL FLOW", times_ms, "feats", extra_stats);
+
+ //=====================================================================================
+ //=====================================================================================
+ //=====================================================================================
+
+ // EIGEN3(double): RANDOM BIG MATRIX
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen1, big_matrix_eigen1);
+ Eigen::MatrixXd mat2 = Eigen::MatrixXd::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): RANDOM BIG MATRIX", times_ms);
+
+ // EIGEN3(double): MATRIX MULTIPLY
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen1, big_matrix_eigen1);
+ Eigen::MatrixXd mat2 = Eigen::MatrixXd::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXd mat3 = mat1 * mat2;
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): MATRIX MULTIPLY", times_ms);
+
+ // EIGEN3(double): MATRIX INVERSION
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ mat1 = mat1.inverse();
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): MATRIX INVERSION", times_ms);
+
+ // EIGEN3(double): HOUSEHOLDER QR FULL
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen2, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXd QR = mat1.householderQr().matrixQR();
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): HOUSEHOLDER QR FULL", times_ms);
+
+ // EIGEN3(double): HOUSEHOLDER QR PIV
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen2, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXd R = mat1.colPivHouseholderQr().matrixR();
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): HOUSEHOLDER QR PIV", times_ms);
+
+ // EIGEN3(double): HOUSEHOLDER QR CUSTOM
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXd mat1 = Eigen::MatrixXd::Random(big_matrix_eigen2, big_matrix_eigen1);
+ Eigen::VectorXd tempV1 = Eigen::VectorXd::Zero(big_matrix_eigen2 * big_matrix_eigen1, 1);
+ Eigen::VectorXd tempV2;
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ for (int k = 0; k < mat1.cols(); k++) {
+ int rows_left = mat1.rows() - k;
+ double beta, tau;
+ mat1.col(k).segment(k, rows_left).makeHouseholder(tempV2, tau, beta);
+ mat1.block(k, k, rows_left, mat1.cols() - k).applyHouseholderOnTheLeft(tempV2, tau, tempV1.data());
+ }
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(double): HOUSEHOLDER QR CUSTOM", times_ms);
+
+ //=====================================================================================
+ //=====================================================================================
+ //=====================================================================================
+
+ // EIGEN3(float): RANDOM BIG MATRIX
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen1, big_matrix_eigen1);
+ Eigen::MatrixXf mat2 = Eigen::MatrixXf::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): RANDOM BIG MATRIX", times_ms);
+
+ // EIGEN3(float): MATRIX MULTIPLY
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen1, big_matrix_eigen1);
+ Eigen::MatrixXf mat2 = Eigen::MatrixXf::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXf mat3 = mat1 * mat2;
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): MATRIX MULTIPLY", times_ms);
+
+ // EIGEN3(float): MATRIX INVERSION
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen1, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ mat1 = mat1.inverse();
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): MATRIX INVERSION", times_ms);
+
+ // EIGEN3(float): HOUSEHOLDER QR FULL
+ times_ms.clear();
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen2, big_matrix_eigen1);
+ for (int i = 0; i < num_trials; i++) {
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXf Q = mat1.householderQr().householderQ();
+ Eigen::MatrixXf R = Q.transpose() * mat1;
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): HOUSEHOLDER QR FULL", times_ms);
+
+ // EIGEN3(float): HOUSEHOLDER QR PIV
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen2, big_matrix_eigen1);
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ Eigen::MatrixXf R = mat1.colPivHouseholderQr().matrixR().triangularView();
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): HOUSEHOLDER QR PIV", times_ms);
+
+ // EIGEN3(float): HOUSEHOLDER QR CUSTOM
+ times_ms.clear();
+ for (int i = 0; i < num_trials; i++) {
+ Eigen::MatrixXf mat1 = Eigen::MatrixXf::Random(big_matrix_eigen2, big_matrix_eigen1);
+ Eigen::VectorXf tempV1 = Eigen::VectorXf::Zero(big_matrix_eigen2 * big_matrix_eigen1, 1);
+ Eigen::VectorXf tempV2;
+ auto rT1 = boost::posix_time::microsec_clock::local_time();
+ for (int k = 0; k < mat1.cols(); k++) {
+ int rows_left = mat1.rows() - k;
+ float beta, tau;
+ mat1.col(k).segment(k, rows_left).makeHouseholder(tempV2, tau, beta);
+ mat1.block(k, k, rows_left, mat1.cols() - k).applyHouseholderOnTheLeft(tempV2, tau, tempV1.data());
+ }
+ auto rT2 = boost::posix_time::microsec_clock::local_time();
+ times_ms.push_back((rT2 - rT1).total_microseconds() * 1e-3);
+ }
+ print_stats("EIGEN3(float): HOUSEHOLDER QR CUSTOM", times_ms);
+
+ // Done!
+ return EXIT_SUCCESS;
+}
diff --git a/ov_core/src/test_tracking.cpp b/ov_core/src/test_tracking.cpp
index 566c1c025..2d869044a 100644
--- a/ov_core/src/test_tracking.cpp
+++ b/ov_core/src/test_tracking.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/test_webcam.cpp b/ov_core/src/test_webcam.cpp
index 7611cb80d..6728617dc 100644
--- a/ov_core/src/test_webcam.cpp
+++ b/ov_core/src/test_webcam.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h
index 3039f8c61..00680caa8 100644
--- a/ov_core/src/track/Grider_FAST.h
+++ b/ov_core/src/track/Grider_FAST.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/Grider_GRID.h b/ov_core/src/track/Grider_GRID.h
index 0e4f7ff7e..59cede9b8 100644
--- a/ov_core/src/track/Grider_GRID.h
+++ b/ov_core/src/track/Grider_GRID.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp
index 95880b32e..b3035e628 100644
--- a/ov_core/src/track/TrackAruco.cpp
+++ b/ov_core/src/track/TrackAruco.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -161,9 +161,10 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t
rT3 = boost::posix_time::microsec_clock::local_time();
// Timing information
- // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n",(rT3-rT2).total_microseconds() * 1e-6,
- // (int)good_left.size()); PRINT_DEBUG("[TIME-ARUCO]: %.4f seconds for total\n",(rT3-rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-ARUCO]: %.4f seconds for detection\n", (rT2 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-ARUCO]: %.4f seconds for feature DB update (%d features)\n", (rT3 - rT2).total_microseconds() * 1e-6,
+ (int)ids_new.size());
+ PRINT_ALL("[TIME-ARUCO]: %.4f seconds for total\n", (rT3 - rT1).total_microseconds() * 1e-6);
}
void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2, int g2, int b2, std::string overlay) {
diff --git a/ov_core/src/track/TrackAruco.h b/ov_core/src/track/TrackAruco.h
index 367a0acdc..eb3651506 100644
--- a/ov_core/src/track/TrackAruco.h
+++ b/ov_core/src/track/TrackAruco.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/TrackBase.cpp b/ov_core/src/track/TrackBase.cpp
index 9b74b24eb..e214c233f 100644
--- a/ov_core/src/track/TrackBase.cpp
+++ b/ov_core/src/track/TrackBase.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -94,8 +94,8 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
cv::circle(img_temp, pt_l, (is_small) ? 1 : 2, cv::Scalar(r1, g1, b1), cv::FILLED);
// cv::putText(img_out, std::to_string(ids_left_last.at(i)), pt_l, cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(0,0,255),1,cv::LINE_AA);
// Draw rectangle around the point
- cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 5, pt_l.y - 5);
- cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 5, pt_l.y + 5);
+ cv::Point2f pt_l_top = cv::Point2f(pt_l.x - 3, pt_l.y - 3);
+ cv::Point2f pt_l_bot = cv::Point2f(pt_l.x + 3, pt_l.y + 3);
cv::rectangle(img_temp, pt_l_top, pt_l_bot, cv::Scalar(r2, g2, b2), 1);
}
// Draw what camera this is
@@ -191,9 +191,9 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2
break;
// Calculate what color we are drawing in
bool is_stereo = (feat.uvs.size() > 1);
- int color_r = (is_stereo ? b2 : r2) - (int)((is_stereo ? b1 : r1) / feat.uvs[pair.first].size() * z);
- int color_g = (is_stereo ? r2 : g2) - (int)((is_stereo ? r1 : g1) / feat.uvs[pair.first].size() * z);
- int color_b = (is_stereo ? g2 : b2) - (int)((is_stereo ? g1 : b1) / feat.uvs[pair.first].size() * z);
+ int color_r = (is_stereo ? b2 : r2) - (int)(1.0 * (is_stereo ? b1 : r1) / feat.uvs[pair.first].size() * z);
+ int color_g = (is_stereo ? r2 : g2) - (int)(1.0 * (is_stereo ? r1 : g1) / feat.uvs[pair.first].size() * z);
+ int color_b = (is_stereo ? g2 : b2) - (int)(1.0 * (is_stereo ? g1 : b1) / feat.uvs[pair.first].size() * z);
// Draw current point
cv::Point2f pt_c(feat.uvs[pair.first].at(z)(0), feat.uvs[pair.first].at(z)(1));
cv::circle(img_temp, pt_c, (is_small) ? 1 : 2, cv::Scalar(color_r, color_g, color_b), cv::FILLED);
diff --git a/ov_core/src/track/TrackBase.h b/ov_core/src/track/TrackBase.h
index fdb9578d6..e0afee1d9 100644
--- a/ov_core/src/track/TrackBase.h
+++ b/ov_core/src/track/TrackBase.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -133,6 +133,18 @@ class TrackBase {
*/
void change_feat_id(size_t id_old, size_t id_new);
+ /// Getter method for active features in the last frame (observations per camera)
+ std::unordered_map> get_last_obs() {
+ std::lock_guard lckv(mtx_last_vars);
+ return pts_last;
+ }
+
+ /// Getter method for active features in the last frame (ids per camera)
+ std::unordered_map> get_last_ids() {
+ std::lock_guard lckv(mtx_last_vars);
+ return ids_last;
+ }
+
/// Getter method for number of active features
int get_num_features() { return num_features; }
diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp
index 934468354..915873d4e 100644
--- a/ov_core/src/track/TrackDescriptor.cpp
+++ b/ov_core/src/track/TrackDescriptor.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -169,11 +169,12 @@ void TrackDescriptor::feed_monocular(const CameraData &message, size_t msg_id) {
rT5 = boost::posix_time::microsec_clock::local_time();
// Our timing information
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6,
- // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for detection\n", (rT2 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for matching\n", (rT3 - rT2).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for merging\n", (rT4 - rT3).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n", (rT5 - rT4).total_microseconds() * 1e-6,
+ (int)good_left.size());
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for total\n", (rT5 - rT1).total_microseconds() * 1e-6);
}
void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
@@ -343,11 +344,12 @@ void TrackDescriptor::feed_stereo(const CameraData &message, size_t msg_id_left,
rT5 = boost::posix_time::microsec_clock::local_time();
// Our timing information
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for detection\n",(rT2-rT1).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for matching\n",(rT3-rT2).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for merging\n",(rT4-rT3).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n",(rT5-rT4).total_microseconds() * 1e-6,
- // (int)good_left.size()); PRINT_DEBUG("[TIME-DESC]: %.4f seconds for total\n",(rT5-rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for detection\n", (rT2 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for matching\n", (rT3 - rT2).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for merging\n", (rT4 - rT3).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for feature DB update (%d features)\n", (rT5 - rT4).total_microseconds() * 1e-6,
+ (int)good_left.size());
+ PRINT_ALL("[TIME-DESC]: %.4f seconds for total\n", (rT5 - rT1).total_microseconds() * 1e-6);
}
void TrackDescriptor::perform_detection_monocular(const cv::Mat &img0, const cv::Mat &mask0, std::vector &pts0,
diff --git a/ov_core/src/track/TrackDescriptor.h b/ov_core/src/track/TrackDescriptor.h
index e36f564b5..30ce9097f 100644
--- a/ov_core/src/track/TrackDescriptor.h
+++ b/ov_core/src/track/TrackDescriptor.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp
index e5b25209c..21cc0325c 100644
--- a/ov_core/src/track/TrackKLT.cpp
+++ b/ov_core/src/track/TrackKLT.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -124,6 +124,7 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) {
// First we should make that the last images have enough features so we can do KLT
// This will "top-off" our number of tracks so always have a constant number
+ int pts_before_detect = (int)pts_last[cam_id].size();
auto pts_left_old = pts_last[cam_id];
auto ids_left_old = ids_last[cam_id];
perform_detection_monocular(img_pyramid_last[cam_id], img_mask_last[cam_id], pts_left_old, ids_left_old);
@@ -188,13 +189,14 @@ void TrackKLT::feed_monocular(const CameraData &message, size_t msg_id) {
}
rT5 = boost::posix_time::microsec_clock::local_time();
- // // Timing information
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n", (rT3 - rT2).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n", (rT5 - rT4).total_microseconds() * 1e-6,
- // (int)good_left.size());
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n", (rT5 - rT1).total_microseconds() * 1e-6);
+ // Timing information
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for detection (%zu detected)\n", (rT3 - rT2).total_microseconds() * 1e-6,
+ (int)pts_last[cam_id].size() - pts_before_detect);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n", (rT5 - rT4).total_microseconds() * 1e-6,
+ (int)good_left.size());
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for total\n", (rT5 - rT1).total_microseconds() * 1e-6);
}
void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t msg_id_right) {
@@ -239,6 +241,7 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
// First we should make that the last images have enough features so we can do KLT
// This will "top-off" our number of tracks so always have a constant number
+ int pts_before_detect = (int)pts_last[cam_id_left].size();
auto pts_left_old = pts_last[cam_id_left];
auto pts_right_old = pts_last[cam_id_right];
auto ids_left_old = ids_last[cam_id_left];
@@ -379,13 +382,14 @@ void TrackKLT::feed_stereo(const CameraData &message, size_t msg_id_left, size_t
rT6 = boost::posix_time::microsec_clock::local_time();
// // Timing information
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for detection\n", (rT3 - rT2).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for stereo klt\n", (rT5 - rT4).total_microseconds() * 1e-6);
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n", (rT6 - rT5).total_microseconds() * 1e-6,
- // (int)good_left.size());
- // PRINT_DEBUG("[TIME-KLT]: %.4f seconds for total\n", (rT6 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for pyramid\n", (rT2 - rT1).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for detection (%d detected)\n", (rT3 - rT2).total_microseconds() * 1e-6,
+ (int)pts_last[cam_id_left].size() - pts_before_detect);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for temporal klt\n", (rT4 - rT3).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for stereo klt\n", (rT5 - rT4).total_microseconds() * 1e-6);
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for feature DB update (%d features)\n", (rT6 - rT5).total_microseconds() * 1e-6,
+ (int)good_left.size());
+ PRINT_ALL("[TIME-KLT]: %.4f seconds for total\n", (rT6 - rT1).total_microseconds() * 1e-6);
}
void TrackKLT::perform_detection_monocular(const std::vector &img0pyr, const cv::Mat &mask0, std::vector &pts0,
diff --git a/ov_core/src/track/TrackKLT.h b/ov_core/src/track/TrackKLT.h
index 46366f88e..f3ed6ea3e 100644
--- a/ov_core/src/track/TrackKLT.h
+++ b/ov_core/src/track/TrackKLT.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/TrackSIM.cpp b/ov_core/src/track/TrackSIM.cpp
index 900a048dd..dc1e6185f 100644
--- a/ov_core/src/track/TrackSIM.cpp
+++ b/ov_core/src/track/TrackSIM.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/track/TrackSIM.h b/ov_core/src/track/TrackSIM.h
index b6db7b1f2..12ab5c1e9 100644
--- a/ov_core/src/track/TrackSIM.h
+++ b/ov_core/src/track/TrackSIM.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/IMU.h b/ov_core/src/types/IMU.h
index 79b83b35a..fd8b74d13 100644
--- a/ov_core/src/types/IMU.h
+++ b/ov_core/src/types/IMU.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/JPLQuat.h b/ov_core/src/types/JPLQuat.h
index 3085347d3..43e546e63 100644
--- a/ov_core/src/types/JPLQuat.h
+++ b/ov_core/src/types/JPLQuat.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/Landmark.cpp b/ov_core/src/types/Landmark.cpp
index 0d88ac43e..4d20e4e74 100644
--- a/ov_core/src/types/Landmark.cpp
+++ b/ov_core/src/types/Landmark.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/Landmark.h b/ov_core/src/types/Landmark.h
index c4e8ba720..e3965435e 100644
--- a/ov_core/src/types/Landmark.h
+++ b/ov_core/src/types/Landmark.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -60,6 +60,9 @@ class Landmark : public Vec {
/// Boolean if this landmark should be marginalized out
bool should_marg = false;
+ /// Number of times the update has failed for this feature (we should remove if it fails a couple times!)
+ int update_fail_count = 0;
+
/// First normalized uv coordinate bearing of this measurement (used for single depth representation)
Eigen::Vector3d uv_norm_zero;
diff --git a/ov_core/src/types/LandmarkRepresentation.h b/ov_core/src/types/LandmarkRepresentation.h
index 1ff9f80a1..43992f10c 100644
--- a/ov_core/src/types/LandmarkRepresentation.h
+++ b/ov_core/src/types/LandmarkRepresentation.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/PoseJPL.h b/ov_core/src/types/PoseJPL.h
index 6676fdfe3..ec11df5ad 100644
--- a/ov_core/src/types/PoseJPL.h
+++ b/ov_core/src/types/PoseJPL.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/Type.h b/ov_core/src/types/Type.h
index 09e4725f9..613572505 100644
--- a/ov_core/src/types/Type.h
+++ b/ov_core/src/types/Type.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/types/Vec.h b/ov_core/src/types/Vec.h
index 57f023aab..a9bc41903 100644
--- a/ov_core/src/types/Vec.h
+++ b/ov_core/src/types/Vec.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/colors.h b/ov_core/src/utils/colors.h
index a4f6c6b47..0c34a022a 100644
--- a/ov_core/src/utils/colors.h
+++ b/ov_core/src/utils/colors.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/dataset_reader.h b/ov_core/src/utils/dataset_reader.h
index b9b305370..01589e547 100644
--- a/ov_core/src/utils/dataset_reader.h
+++ b/ov_core/src/utils/dataset_reader.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/opencv_lambda_body.h b/ov_core/src/utils/opencv_lambda_body.h
index 1ac9d2190..a4c85bb5e 100644
--- a/ov_core/src/utils/opencv_lambda_body.h
+++ b/ov_core/src/utils/opencv_lambda_body.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/opencv_yaml_parse.h b/ov_core/src/utils/opencv_yaml_parse.h
index 30769da15..fd2d3921f 100644
--- a/ov_core/src/utils/opencv_yaml_parse.h
+++ b/ov_core/src/utils/opencv_yaml_parse.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/print.cpp b/ov_core/src/utils/print.cpp
index 9dcdc084d..49ab68493 100644
--- a/ov_core/src/utils/print.cpp
+++ b/ov_core/src/utils/print.cpp
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/print.h b/ov_core/src/utils/print.h
index 00fa120e8..94e842df2 100644
--- a/ov_core/src/utils/print.h
+++ b/ov_core/src/utils/print.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
@@ -99,15 +99,4 @@ class Printer {
#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x);
#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x);
-// Assert that will always be here in release builds also
-// TODO: place this in a better header, just putting here for now...
-#define assert_r(EX) (void)((EX) || (__assert(#EX, __FILE__, __LINE__), 0))
-#ifdef __cplusplus
-extern "C" {
-#endif
-extern void __assert(const char *msg, const char *file, int line);
-#ifdef __cplusplus
-};
-#endif
-
#endif /* OV_CORE_PRINT_H */
diff --git a/ov_core/src/utils/quat_ops.h b/ov_core/src/utils/quat_ops.h
index ad421cce1..83ce3c211 100644
--- a/ov_core/src/utils/quat_ops.h
+++ b/ov_core/src/utils/quat_ops.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_core/src/utils/sensor_data.h b/ov_core/src/utils/sensor_data.h
index 28ff9052b..17d01f345 100644
--- a/ov_core/src/utils/sensor_data.h
+++ b/ov_core/src/utils/sensor_data.h
@@ -1,8 +1,8 @@
/*
* OpenVINS: An Open Platform for Visual-Inertial Research
- * Copyright (C) 2018-2022 Patrick Geneva
- * Copyright (C) 2018-2022 Guoquan Huang
- * Copyright (C) 2018-2022 OpenVINS Contributors
+ * Copyright (C) 2018-2023 Patrick Geneva
+ * Copyright (C) 2018-2023 Guoquan Huang
+ * Copyright (C) 2018-2023 OpenVINS Contributors
* Copyright (C) 2018-2019 Kevin Eckenhoff
*
* This program is free software: you can redistribute it and/or modify
diff --git a/ov_data/kaist/matlab/kaist_calculate_imu2cam.m b/ov_data/kaist/matlab/kaist_calculate_imu2cam.m
index 2a14c2ba6..be05aad54 100644
--- a/ov_data/kaist/matlab/kaist_calculate_imu2cam.m
+++ b/ov_data/kaist/matlab/kaist_calculate_imu2cam.m
@@ -3,11 +3,12 @@
close all;
+% stereoParams.mat
% load their stereo parameters
% looks like they used the matlab calibration tool to get it.
% https://www.mathworks.com/help/vision/ref/stereoparameters.html#d117e94134
load ../raw/stereoParams.mat
-%stereoParams = toStruct(stereoParams);
+stereoParams = toStruct(stereoParams);
% transform between the left (stereo pair base) and the right camera
% the units of the stereo parameters are millimeters
@@ -19,21 +20,23 @@
0 0 0 1
];
-
-% T_LtoR = zeros(4,4);
-% T_LtoR(1,1) = 1;
-% T_LtoR(1,4) = -0.4751;
-% T_LtoR(2,2) = 1;
-% T_LtoR(3,3) = 1;
-% T_LtoR(4,4) = 1;
+% Use this computed output if you do not have the toolbox
+% TODO: can we manually read in the stereParams.mat? (probs not)
+% T_LtoR = [...
+% 1.0000 -0.0021 -0.0036 -0.4751;
+% 0.0021 1.0000 0.0046 -0.0011;
+% 0.0036 -0.0046 1.0000 0.0020;
+% 0 0 0 1.0000;
+% ];
% Vehicle2Stereo.txt
% Stereo camera (based on left camera) extrinsic calibration parameter from vehicle
-% WHAT????? THEIR POSITION IS INCORRECTLY THE OPOSITE
-% DIRECTION????!@#!@#?!@#!@#$!@$%!#@%!#%!~@%!@#%@!#^%#@%^!@#%@#%
-p_LinV = [1.66944; 0.278027; 1.61215];
-R_LtoV = [-0.00413442 -0.0196634 0.999798; -0.999931 -0.0109505 -0.00435034; 0.0110338 -0.999747 -0.0196168];
+% NOTE: This was taken from the Urban28 dataset
+% NOTE: The sample_with_img.tar.gz has different calibration (shouldn't be used)
+% WHAT????? THEIR POSITION IS INCORRECTLY THE OPOSITE DIRECTION????
+p_LinV = [1.64239; 0.247401; 1.58411];
+R_LtoV = [-0.00680499 -0.0153215 0.99985; -0.999977 0.000334627 -0.00680066; -0.000230383 -0.999883 -0.0153234];
T_VtoL = [...
R_LtoV' -R_LtoV'*p_LinV;
0 0 0 1
@@ -50,7 +53,6 @@
];
-
% calculate the transform between the camneras and the IMU
T_LtoI = T_VtoI*inv(T_VtoL);
T_RtoI = T_VtoI*inv(T_LtoR*T_VtoL);
diff --git a/ov_data/package.xml b/ov_data/package.xml
index 8cb3887dd..0ec169cd4 100644
--- a/ov_data/package.xml
+++ b/ov_data/package.xml
@@ -3,7 +3,7 @@
ov_data
- 2.6.0
+ 2.6.3
Data for the OpenVINS project, mostly just groundtruth files...