From aaa82fba3c09562fbedc7038cf02df5b16da599b Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Mon, 12 Aug 2019 19:14:47 +0200 Subject: [PATCH] Added LM options --- data/euroc_config.json | 8 +- data/tumvi_512_config.json | 8 +- include/basalt/utils/vio_config.h | 8 ++ src/utils/vio_config.cpp | 16 +++ src/vi_estimator/keypoint_vio.cpp | 161 ++++++++++++++++++------------ 5 files changed, 137 insertions(+), 64 deletions(-) diff --git a/data/euroc_config.json b/data/euroc_config.json index dcb40ff..1804a50 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -20,6 +20,9 @@ "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, @@ -33,6 +36,9 @@ "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_no_factor_weights": false, + "config.mapper_use_lm": false, + "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 4ba4452..4210b70 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -21,6 +21,9 @@ "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, @@ -34,6 +37,9 @@ "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_no_factor_weights": false, + "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 6fd4ce4..b14be19 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -68,6 +68,10 @@ struct VioConfig { bool vio_enforce_realtime; + bool vio_use_lm; + double vio_lm_lambda_min; + double vio_lm_lambda_max; + double mapper_obs_std_dev; double mapper_obs_huber_thresh; int mapper_detection_num_points; @@ -81,5 +85,9 @@ struct VioConfig { int mapper_bow_num_bits; double mapper_min_triangulation_dist; bool mapper_no_factor_weights; + + bool mapper_use_lm; + double mapper_lm_lambda_min; + double mapper_lm_lambda_max; }; } // namespace basalt diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 6ed56a1..eea0815 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -68,6 +68,10 @@ VioConfig::VioConfig() { vio_enforce_realtime = false; + vio_use_lm = false; + vio_lm_lambda_min = 1e-32; + vio_lm_lambda_max = 1e2; + mapper_obs_std_dev = 0.25; mapper_obs_huber_thresh = 1.5; mapper_detection_num_points = 800; @@ -81,6 +85,10 @@ VioConfig::VioConfig() { mapper_bow_num_bits = 16; mapper_min_triangulation_dist = 0.07; mapper_no_factor_weights = false; + + mapper_use_lm = false; + mapper_lm_lambda_min = 1e-32; + mapper_lm_lambda_max = 1e2; } void VioConfig::save(const std::string& filename) { @@ -132,6 +140,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.vio_enforce_realtime)); + ar(CEREAL_NVP(config.vio_use_lm)); + ar(CEREAL_NVP(config.vio_lm_lambda_min)); + ar(CEREAL_NVP(config.vio_lm_lambda_max)); + ar(CEREAL_NVP(config.mapper_obs_std_dev)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_detection_num_points)); @@ -145,5 +157,9 @@ 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_lm)); + ar(CEREAL_NVP(config.mapper_lm_lambda_min)); + ar(CEREAL_NVP(config.mapper_lm_lambda_max)); } } // namespace cereal diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index c50c5dd..9941f75 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -54,9 +54,9 @@ KeypointVioEstimator::KeypointVioEstimator( g(g), initialized(false), config(config), - lambda(1e-10), - min_lambda(1e-32), - max_lambda(100), + lambda(config.vio_lm_lambda_min), + min_lambda(config.vio_lm_lambda_min), + max_lambda(config.vio_lm_lambda_max), lambda_vee(2) { this->obs_std_dev = config.vio_obs_std_dev; this->huber_thresh = config.vio_obs_huber_thresh; @@ -945,18 +945,107 @@ void KeypointVioEstimator::optimize() { double error_total = rld_error + imu_error + marg_prior_error + ba_error + bg_error; - std::cout << "[LINEARIZE] Error: " << error_total << " num points " - << std::endl; + if (config.vio_debug) + std::cout << "[LINEARIZE] Error: " << error_total << " num points " + << std::endl; lopt.accum.setup_solver(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); bool converged = false; - bool step = false; - int max_iter = 10; - while (!step && max_iter > 0 && !converged) { - Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + if (config.vio_use_lm) { // Use Levenberg–Marquardt + bool step = false; + int max_iter = 10; + + while (!step && max_iter > 0 && !converged) { + Eigen::VectorXd Hdiag_lambda = Hdiag * lambda; + for (int i = 0; i < Hdiag_lambda.size(); i++) + Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); + + const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda); + double max_inc = inc.array().abs().maxCoeff(); + if (max_inc < 1e-4) converged = true; + + backup(); + + // apply increment to poses + for (auto& kv : frame_poses) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // apply increment to states + for (auto& kv : frame_states) { + int idx = aom.abs_order_map.at(kv.first).first; + kv.second.applyInc(-inc.segment(idx)); + } + + // Update points + tbb::blocked_range keys_range(0, rld_vec.size()); + auto update_points_func = [&](const tbb::blocked_range& r) { + for (size_t i = r.begin(); i != r.end(); ++i) { + const auto& rld = rld_vec[i]; + updatePoints(aom, rld, inc); + } + }; + tbb::parallel_for(keys_range, update_points_func); + + double after_update_marg_prior_error = 0; + double after_update_vision_error = 0, after_update_imu_error = 0, + after_bg_error = 0, after_ba_error = 0; + + computeError(after_update_vision_error); + computeImuError(aom, after_update_imu_error, after_bg_error, + after_ba_error, frame_states, imu_meas, + gyro_bias_weight, accel_bias_weight, g); + computeMargPriorError(after_update_marg_prior_error); + + double after_error_total = + after_update_vision_error + after_update_imu_error + + after_update_marg_prior_error + after_bg_error + after_ba_error; + + double f_diff = (error_total - after_error_total); + double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB()); + + // std::cout << "f_diff " << f_diff << " l_diff " << l_diff << + // std::endl; + + double step_quality = f_diff / l_diff; + + if (step_quality < 0) { + if (config.vio_debug) + std::cout << "\t[REJECTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " Error: " << after_error_total << std::endl; + lambda = std::min(max_lambda, lambda_vee * lambda); + lambda_vee *= 2; + + restore(); + } else { + if (config.vio_debug) + std::cout << "\t[ACCEPTED] lambda:" << lambda + << " step_quality: " << step_quality + << " max_inc: " << max_inc + << " Error: " << after_error_total << std::endl; + + lambda = std::max( + min_lambda, + lambda * + std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0))); + lambda_vee = 2; + + step = true; + } + max_iter--; + } + + if (config.vio_debug && converged) { + std::cout << "[CONVERGED]" << std::endl; + } + } else { // Use Gauss-Newton + Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda; for (int i = 0; i < Hdiag_lambda.size(); i++) Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda); @@ -964,8 +1053,6 @@ void KeypointVioEstimator::optimize() { double max_inc = inc.array().abs().maxCoeff(); if (max_inc < 1e-4) converged = true; - backup(); - // apply increment to poses for (auto& kv : frame_poses) { int idx = aom.abs_order_map.at(kv.first).first; @@ -987,56 +1074,6 @@ void KeypointVioEstimator::optimize() { } }; tbb::parallel_for(keys_range, update_points_func); - - double after_update_marg_prior_error = 0; - double after_update_vision_error = 0, after_update_imu_error = 0, - after_bg_error = 0, after_ba_error = 0; - - computeError(after_update_vision_error); - computeImuError(aom, after_update_imu_error, after_bg_error, - after_ba_error, frame_states, imu_meas, - gyro_bias_weight, accel_bias_weight, g); - computeMargPriorError(after_update_marg_prior_error); - - double after_error_total = - after_update_vision_error + after_update_imu_error + - after_update_marg_prior_error + after_bg_error + after_ba_error; - - double f_diff = (error_total - after_error_total); - double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB()); - - std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl; - - double step_quality = f_diff / l_diff; - - if (step_quality < 0) { - std::cout << "\t[REJECTED] lambda:" << lambda - << " step_quality: " << step_quality - << " max_inc: " << max_inc - << " Error: " << after_error_total << std::endl; - lambda = std::min(max_lambda, lambda_vee * lambda); - lambda_vee *= 2; - - restore(); - } else { - std::cout << "\t[ACCEPTED] lambda:" << lambda - << " step_quality: " << step_quality - << " max_inc: " << max_inc - << " Error: " << after_error_total << std::endl; - - lambda = std::max( - min_lambda, - lambda * - std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0))); - lambda_vee = 2; - - step = true; - } - max_iter--; - } - - if (converged) { - std::cout << "[CONVERGED]" << std::endl; } if (config.vio_debug) { @@ -1099,7 +1136,7 @@ void KeypointVioEstimator::optimize() { if (config.vio_debug) { std::cout << "=================================" << std::endl; } -} +} // namespace basalt void KeypointVioEstimator::computeProjections( std::vector>& data) const {