added enforce_realtime option

This commit is contained in:
Vladyslav Usenko 2019-08-04 22:54:22 +02:00
parent 254fc0bf33
commit 56cad21c6f
6 changed files with 14 additions and 2 deletions

View File

@ -19,7 +19,7 @@
"config.vio_outlier_threshold": 3.0, "config.vio_outlier_threshold": 3.0,
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false,
"config.mapper_obs_std_dev": 0.25, "config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -19,7 +19,7 @@
"config.vio_outlier_threshold": 3.0, "config.vio_outlier_threshold": 3.0,
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false,
"config.mapper_obs_std_dev": 0.25, "config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -20,6 +20,7 @@
"config.vio_outlier_threshold": 3.0, "config.vio_outlier_threshold": 3.0,
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false,
"config.mapper_obs_std_dev": 0.25, "config.mapper_obs_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,

View File

@ -66,6 +66,8 @@ struct VioConfig {
double vio_obs_huber_thresh; double vio_obs_huber_thresh;
double vio_min_triangulation_dist; double vio_min_triangulation_dist;
bool vio_enforce_realtime;
double mapper_obs_std_dev; double mapper_obs_std_dev;
double mapper_obs_huber_thresh; double mapper_obs_huber_thresh;
int mapper_detection_num_points; int mapper_detection_num_points;

View File

@ -66,6 +66,8 @@ VioConfig::VioConfig() {
vio_filter_iteration = 4; vio_filter_iteration = 4;
vio_max_iterations = 7; vio_max_iterations = 7;
vio_enforce_realtime = false;
mapper_obs_std_dev = 0.25; mapper_obs_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5; mapper_obs_huber_thresh = 1.5;
mapper_detection_num_points = 800; 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_obs_huber_thresh));
ar(CEREAL_NVP(config.vio_min_triangulation_dist)); 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_std_dev));
ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
ar(CEREAL_NVP(config.mapper_detection_num_points)); ar(CEREAL_NVP(config.mapper_detection_num_points));

View File

@ -125,6 +125,11 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
while (true) { while (true) {
vision_data_queue.pop(curr_frame); 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()) { if (!curr_frame.get()) {
break; break;
} }