diff --git a/data/euroc_config.json b/data/euroc_config.json index 2fee0d4..dcb40ff 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -19,7 +19,7 @@ "config.vio_outlier_threshold": 3.0, "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, - + "config.vio_enforce_realtime": false, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, diff --git a/data/euroc_config_no_weights.json b/data/euroc_config_no_weights.json index 76f377c..91231c1 100644 --- a/data/euroc_config_no_weights.json +++ b/data/euroc_config_no_weights.json @@ -19,7 +19,7 @@ "config.vio_outlier_threshold": 3.0, "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, - + "config.vio_enforce_realtime": false, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index de31800..4ba4452 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -20,6 +20,7 @@ "config.vio_outlier_threshold": 3.0, "config.vio_filter_iteration": 4, "config.vio_max_iterations": 7, + "config.vio_enforce_realtime": false, "config.mapper_obs_std_dev": 0.25, "config.mapper_obs_huber_thresh": 1.5, diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 6d79528..6fd4ce4 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -66,6 +66,8 @@ struct VioConfig { double vio_obs_huber_thresh; double vio_min_triangulation_dist; + bool vio_enforce_realtime; + double mapper_obs_std_dev; double mapper_obs_huber_thresh; int mapper_detection_num_points; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 9b7c272..6ed56a1 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -66,6 +66,8 @@ VioConfig::VioConfig() { vio_filter_iteration = 4; vio_max_iterations = 7; + vio_enforce_realtime = false; + mapper_obs_std_dev = 0.25; mapper_obs_huber_thresh = 1.5; mapper_detection_num_points = 800; @@ -128,6 +130,8 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.vio_obs_huber_thresh)); ar(CEREAL_NVP(config.vio_min_triangulation_dist)); + ar(CEREAL_NVP(config.vio_enforce_realtime)); + ar(CEREAL_NVP(config.mapper_obs_std_dev)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_detection_num_points)); diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 95b5a70..f413019 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -125,6 +125,11 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, while (true) { vision_data_queue.pop(curr_frame); + if (config.vio_enforce_realtime) { + // drop current frame if another frame is already in the queue. + while (!vision_data_queue.empty()) vision_data_queue.pop(curr_frame); + } + if (!curr_frame.get()) { break; }