ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added more config options

This commit is contained in:
Vladyslav Usenko 2019-07-25 14:25:47 +02:00
parent 5b01a126b7
commit c4d73787f0
6 changed files with 36 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
data/euroc_config.json
View File

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

7
data/tumvi_512_config.json
View File

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

3
include/basalt/utils/vio_config.h
View File

@ -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

6
src/utils/vio_config.cpp
View File

@ -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

4
src/vi_estimator/keypoint_vio.cpp
View File

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

13
src/vi_estimator/nfr_mapper.cpp
View File

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