From fc0800514b1387cb0490545398d4e455bfb832dd Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 14 Aug 2019 11:31:12 +0200 Subject: [PATCH] Added pure BA option --- data/euroc_config.json | 3 +- data/euroc_config_no_factors.json | 45 ++++++++++++++++++++++++++++ data/euroc_config_no_weights.json | 3 +- data/tumvi_512_config.json | 1 + include/basalt/utils/vio_config.h | 1 + scripts/eval_full/gen_results.py | 16 ++++++++++ scripts/eval_full/run_evaluations.sh | 3 ++ src/utils/vio_config.cpp | 2 ++ src/vi_estimator/nfr_mapper.cpp | 13 +++++--- 9 files changed, 81 insertions(+), 6 deletions(-) create mode 100644 data/euroc_config_no_factors.json diff --git a/data/euroc_config.json b/data/euroc_config.json index 1804a50..ec47ec1 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -37,7 +37,8 @@ "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, "config.mapper_no_factor_weights": false, - "config.mapper_use_lm": false, + "config.mapper_use_factors": true, + "config.mapper_use_lm": true, "config.mapper_lm_lambda_min": 1e-32, "config.mapper_lm_lambda_max": 1e3 } diff --git a/data/euroc_config_no_factors.json b/data/euroc_config_no_factors.json new file mode 100644 index 0000000..59391aa --- /dev/null +++ b/data/euroc_config_no_factors.json @@ -0,0 +1,45 @@ +{ + "value0": { + "config.optical_flow_type": "frame_to_frame", + "config.optical_flow_detection_grid_size": 50, + "config.optical_flow_max_recovered_dist2": 0.04, + "config.optical_flow_pattern": 51, + "config.optical_flow_max_iterations": 5, + "config.optical_flow_epipolar_error": 0.005, + "config.optical_flow_levels": 3, + "config.optical_flow_skip_frames": 1, + "config.vio_max_states": 3, + "config.vio_max_kfs": 7, + "config.vio_min_frames_after_kf": 5, + "config.vio_new_kf_keypoints_thresh": 0.7, + "config.vio_debug": false, + "config.vio_obs_std_dev": 0.5, + "config.vio_obs_huber_thresh": 1.0, + "config.vio_min_triangulation_dist": 0.05, + "config.vio_outlier_threshold": 3.0, + "config.vio_filter_iteration": 4, + "config.vio_max_iterations": 7, + "config.vio_enforce_realtime": false, + "config.vio_use_lm": false, + "config.vio_lm_lambda_min": 1e-32, + "config.vio_lm_lambda_max": 1e2, + + "config.mapper_obs_std_dev": 0.25, + "config.mapper_obs_huber_thresh": 1.5, + "config.mapper_detection_num_points": 800, + "config.mapper_num_frames_to_match": 30, + "config.mapper_frames_to_match_threshold": 0.04, + "config.mapper_min_matches": 20, + "config.mapper_ransac_threshold": 5e-5, + "config.mapper_min_track_length": 5, + "config.mapper_max_hamming_distance": 70, + "config.mapper_second_best_test_ratio": 1.2, + "config.mapper_bow_num_bits": 16, + "config.mapper_min_triangulation_dist": 0.07, + "config.mapper_no_factor_weights": false, + "config.mapper_use_factors": false, + "config.mapper_use_lm": true, + "config.mapper_lm_lambda_min": 1e-32, + "config.mapper_lm_lambda_max": 1e3 + } +} diff --git a/data/euroc_config_no_weights.json b/data/euroc_config_no_weights.json index 8743e73..844aaac 100644 --- a/data/euroc_config_no_weights.json +++ b/data/euroc_config_no_weights.json @@ -37,7 +37,8 @@ "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, "config.mapper_no_factor_weights": true, - "config.mapper_use_lm": false, + "config.mapper_use_factors": true, + "config.mapper_use_lm": true, "config.mapper_lm_lambda_min": 1e-32, "config.mapper_lm_lambda_max": 1e3 } diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index 4210b70..ca9bd6e 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -38,6 +38,7 @@ "config.mapper_bow_num_bits": 16, "config.mapper_min_triangulation_dist": 0.07, "config.mapper_no_factor_weights": false, + "config.mapper_use_factors": true, "config.mapper_use_lm": false, "config.mapper_lm_lambda_min": 1e-32, "config.mapper_lm_lambda_max": 1e3 diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index b14be19..dcad82a 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -85,6 +85,7 @@ struct VioConfig { int mapper_bow_num_bits; double mapper_min_triangulation_dist; bool mapper_no_factor_weights; + bool mapper_use_factors; bool mapper_use_lm; double mapper_lm_lambda_min; diff --git a/scripts/eval_full/gen_results.py b/scripts/eval_full/gen_results.py index 07f16af..c459f07 100755 --- a/scripts/eval_full/gen_results.py +++ b/scripts/eval_full/gen_results.py @@ -31,6 +31,12 @@ pose_graph = { 'num_frames' : ['PG Num. KFs'] } +pure_ba = { +'ate' : ['PG RMS ATE [m]'], +'time' : ['PG Time [s]'], +'num_frames' : ['PG Num. KFs'] +} + out_dir = sys.argv[1] def load_data(x, prefix, key): @@ -52,6 +58,7 @@ for key in datasets[1:]: load_data(vio, 'vio', key) load_data(mapping, 'mapper', key) load_data(pose_graph, 'mapper_no_weights', key) + load_data(pure_ba, 'mapper_no_factors', key) row_format ="{:>17}" + "{:>13}" * (len(datasets)-1) @@ -80,3 +87,12 @@ print row_format.format(*pose_graph['ate']) #print row_format.format(*pose_graph['time']) print row_format.format(*pose_graph['num_frames']) + +print '\nPure BA optimization (no factors from the recovery used)' +print row_format.format(*datasets_short) + +print row_format.format(*pure_ba['ate']) +#print row_format.format(*pure_ba['time']) +print row_format.format(*pure_ba['num_frames']) + + diff --git a/scripts/eval_full/run_evaluations.sh b/scripts/eval_full/run_evaluations.sh index 833f849..c507174 100755 --- a/scripts/eval_full/run_evaluations.sh +++ b/scripts/eval_full/run_evaluations.sh @@ -24,6 +24,9 @@ for d in ${DATASETS[$CI_NODE_INDEX-1]}; do basalt_mapper --show-gui 0 --cam-calib /usr/etc/basalt/euroc_eucm_calib.json --config-path /usr/etc/basalt/euroc_config_no_weights.json --marg-data eval_tmp_marg_data \ --result-path $folder_name/mapper_no_weights_$d + basalt_mapper --show-gui 0 --cam-calib /usr/etc/basalt/euroc_eucm_calib.json --config-path /usr/etc/basalt/euroc_config_no_factors.json --marg-data eval_tmp_marg_data \ + --result-path $folder_name/mapper_no_factors_$d + rm -rf eval_tmp_marg_data done diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index eea0815..ab630c1 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -85,6 +85,7 @@ VioConfig::VioConfig() { mapper_bow_num_bits = 16; mapper_min_triangulation_dist = 0.07; mapper_no_factor_weights = false; + mapper_use_factors = true; mapper_use_lm = false; mapper_lm_lambda_min = 1e-32; @@ -157,6 +158,7 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.mapper_bow_num_bits)); ar(CEREAL_NVP(config.mapper_min_triangulation_dist)); ar(CEREAL_NVP(config.mapper_no_factor_weights)); + ar(CEREAL_NVP(config.mapper_use_factors)); ar(CEREAL_NVP(config.mapper_use_lm)); ar(CEREAL_NVP(config.mapper_lm_lambda_min)); diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 84b4344..d2c695c 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -273,8 +273,11 @@ void NfrMapper::optimize(int num_iterations) { rel_pose_factors.begin(), rel_pose_factors.end()); tbb::parallel_reduce(range, lopt); - tbb::parallel_reduce(range1, lopt); - tbb::parallel_reduce(range2, lopt); + + if (config.mapper_use_factors) { + tbb::parallel_reduce(range1, lopt); + tbb::parallel_reduce(range2, lopt); + } double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error; @@ -329,8 +332,10 @@ void NfrMapper::optimize(int num_iterations) { double after_roll_pitch_error = 0; computeError(after_update_vision_error); - computeRelPose(after_rel_error); - computeRollPitch(after_roll_pitch_error); + if (config.mapper_use_factors) { + computeRelPose(after_rel_error); + computeRollPitch(after_roll_pitch_error); + } double after_error_total = after_update_vision_error + after_rel_error + after_roll_pitch_error;