added more config options

data/euroc_config.json

@@ -16,17 +16,20 @@
         "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.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.3,
+        "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_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": true
     }
 }

data/tumvi_512_config.json

@@ -16,17 +16,20 @@
         "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.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.3,
+        "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_bow_num_bits": 16,
+        "config.mapper_min_triangulation_dist": 0.07,
+        "config.mapper_no_factor_weights": false
     }
 }

include/basalt/utils/vio_config.h

@@ -61,6 +61,7 @@ struct VioConfig {
 
   double vio_obs_std_dev;
   double vio_obs_huber_thresh;
+  double vio_min_triangulation_dist;
 
   double mapper_obs_std_dev;
   double mapper_obs_huber_thresh;
@@ -73,5 +74,7 @@ struct VioConfig {
   double mapper_max_hamming_distance;
   double mapper_second_best_test_ratio;
   int mapper_bow_num_bits;
+  double mapper_min_triangulation_dist;
+  bool mapper_no_factor_weights;
 };
 }  // namespace basalt

src/utils/vio_config.cpp

@@ -61,6 +61,7 @@ VioConfig::VioConfig() {
   vio_debug = false;
   vio_obs_std_dev = 0.5;
   vio_obs_huber_thresh = 1.0;
+  vio_min_triangulation_dist = 0.05;
 
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
@@ -73,6 +74,8 @@ VioConfig::VioConfig() {
   mapper_max_hamming_distance = 70;
   mapper_second_best_test_ratio = 1.2;
   mapper_bow_num_bits = 16;
+  mapper_min_triangulation_dist = 0.07;
+  mapper_no_factor_weights = false;
 }
 
 void VioConfig::save(const std::string& filename) {
@@ -118,6 +121,7 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
 
   ar(CEREAL_NVP(config.vio_obs_std_dev));
   ar(CEREAL_NVP(config.vio_obs_huber_thresh));
+  ar(CEREAL_NVP(config.vio_min_triangulation_dist));
 
   ar(CEREAL_NVP(config.mapper_obs_std_dev));
   ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
@@ -130,5 +134,7 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.mapper_max_hamming_distance));
   ar(CEREAL_NVP(config.mapper_second_best_test_ratio));
   ar(CEREAL_NVP(config.mapper_bow_num_bits));
+  ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
+  ar(CEREAL_NVP(config.mapper_no_factor_weights));
 }
 }  // namespace cereal

src/vi_estimator/keypoint_vio.cpp

@@ -305,6 +305,8 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
 
       // triangulate
       bool valid_kp = false;
+      const double min_triang_distance2 =
+          config.vio_min_triangulation_dist * config.vio_min_triangulation_dist;
       for (const auto& kv_obs : kp_obs) {
         if (valid_kp) break;
         TimeCamId tcido = kv_obs.first;
@@ -330,7 +332,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
         Sophus::SE3d T_0_1 =
             calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];
 
-        if (T_0_1.translation().squaredNorm() < 0.05 * 0.05) continue;
+        if (T_0_1.translation().squaredNorm() < min_triang_distance2) continue;
 
         Eigen::Vector4d p0_triangulated =
             triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1);

src/vi_estimator/nfr_mapper.cpp

@@ -182,7 +182,12 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
     RollPitchFactor rpf;
     rpf.t_ns = kf_id;
     rpf.R_w_i_meas = T_w_i_kf.so3();
+
+    if (!config.mapper_no_factor_weights) {
       rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse();
+    } else {
+      rpf.cov_inv.setIdentity();
+    }
 
     roll_pitch_factors.emplace_back(rpf);
   }
@@ -213,7 +218,10 @@ bool NfrMapper::extractNonlinearFactors(MargData& m) {
     rpf.t_j_ns = other_id;
     rpf.T_i_j = T_kf_o;
     rpf.cov_inv.setIdentity();
+
+    if (!config.mapper_no_factor_weights) {
       cov_new.ldlt().solveInPlace(rpf.cov_inv);
+    }
 
     // std::cout << "rpf.cov_inv\n" << rpf.cov_inv << std::endl;
 
@@ -609,6 +617,9 @@ void NfrMapper::build_tracks() {
 }
 
 void NfrMapper::setup_opt() {
+  const double min_triang_distance2 = config.mapper_min_triangulation_dist *
+                                      config.mapper_min_triangulation_dist;
+
   for (const auto& kv : feature_tracks) {
     if (kv.second.size() < 2) continue;
 
@@ -638,7 +649,7 @@ void NfrMapper::setup_opt() {
 
       Sophus::SE3d T_h_o = T_w_h.inverse() * T_w_o;
 
-      if (T_h_o.translation().squaredNorm() < 0.07 * 0.07) continue;
+      if (T_h_o.translation().squaredNorm() < min_triang_distance2) continue;
 
       Eigen::Vector4d pos_3d =
           triangulate(pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_h_o);