Added pure BA option

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
     }

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
+    }
+}

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
     }

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

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;

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'])
+
+

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
 

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));

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);
+
+    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);
+        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;