diff --git a/data/euroc_config.json b/data/euroc_config.json index f085b37..7feeb3f 100644 --- a/data/euroc_config.json +++ b/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 } } diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index 443397e..dc9114f 100644 --- a/data/tumvi_512_config.json +++ b/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 } } diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index ef4f062..eed8667 100644 --- a/include/basalt/utils/vio_config.h +++ b/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 diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index c071dbb..2196372 100644 --- a/src/utils/vio_config.cpp +++ b/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 diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 8dcd369..e49c9db 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/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); diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 8469df2..361cc89 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/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(); - rpf.cov_inv = cov_new.block<2, 2>(4, 4).inverse(); + + 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(); - cov_new.ldlt().solveInPlace(rpf.cov_inv); + + 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);