Added LM options

This commit is contained in:
Vladyslav Usenko 2019-08-12 19:14:47 +02:00
parent 4e4901dfa4
commit aaa82fba3c
5 changed files with 137 additions and 64 deletions

View File

@ -20,6 +20,9 @@
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,
@ -33,6 +36,9 @@
"config.mapper_second_best_test_ratio": 1.2, "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_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
} }
} }

View File

@ -21,6 +21,9 @@
"config.vio_filter_iteration": 4, "config.vio_filter_iteration": 4,
"config.vio_max_iterations": 7, "config.vio_max_iterations": 7,
"config.vio_enforce_realtime": false, "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_std_dev": 0.25,
"config.mapper_obs_huber_thresh": 1.5, "config.mapper_obs_huber_thresh": 1.5,
@ -34,6 +37,9 @@
"config.mapper_second_best_test_ratio": 1.2, "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_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
} }
} }

View File

@ -68,6 +68,10 @@ struct VioConfig {
bool vio_enforce_realtime; 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_std_dev;
double mapper_obs_huber_thresh; double mapper_obs_huber_thresh;
int mapper_detection_num_points; int mapper_detection_num_points;
@ -81,5 +85,9 @@ struct VioConfig {
int mapper_bow_num_bits; int mapper_bow_num_bits;
double mapper_min_triangulation_dist; double mapper_min_triangulation_dist;
bool mapper_no_factor_weights; bool mapper_no_factor_weights;
bool mapper_use_lm;
double mapper_lm_lambda_min;
double mapper_lm_lambda_max;
}; };
} // namespace basalt } // namespace basalt

View File

@ -68,6 +68,10 @@ VioConfig::VioConfig() {
vio_enforce_realtime = false; 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_std_dev = 0.25;
mapper_obs_huber_thresh = 1.5; mapper_obs_huber_thresh = 1.5;
mapper_detection_num_points = 800; mapper_detection_num_points = 800;
@ -81,6 +85,10 @@ VioConfig::VioConfig() {
mapper_bow_num_bits = 16; mapper_bow_num_bits = 16;
mapper_min_triangulation_dist = 0.07; mapper_min_triangulation_dist = 0.07;
mapper_no_factor_weights = false; 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) { 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_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_std_dev));
ar(CEREAL_NVP(config.mapper_obs_huber_thresh)); ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
ar(CEREAL_NVP(config.mapper_detection_num_points)); 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_bow_num_bits));
ar(CEREAL_NVP(config.mapper_min_triangulation_dist)); ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
ar(CEREAL_NVP(config.mapper_no_factor_weights)); 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 } // namespace cereal

View File

@ -54,9 +54,9 @@ KeypointVioEstimator::KeypointVioEstimator(
g(g), g(g),
initialized(false), initialized(false),
config(config), config(config),
lambda(1e-10), lambda(config.vio_lm_lambda_min),
min_lambda(1e-32), min_lambda(config.vio_lm_lambda_min),
max_lambda(100), max_lambda(config.vio_lm_lambda_max),
lambda_vee(2) { lambda_vee(2) {
this->obs_std_dev = config.vio_obs_std_dev; this->obs_std_dev = config.vio_obs_std_dev;
this->huber_thresh = config.vio_obs_huber_thresh; this->huber_thresh = config.vio_obs_huber_thresh;
@ -945,6 +945,7 @@ void KeypointVioEstimator::optimize() {
double error_total = double error_total =
rld_error + imu_error + marg_prior_error + ba_error + bg_error; rld_error + imu_error + marg_prior_error + ba_error + bg_error;
if (config.vio_debug)
std::cout << "[LINEARIZE] Error: " << error_total << " num points " std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl; << std::endl;
@ -952,6 +953,8 @@ void KeypointVioEstimator::optimize() {
Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal(); Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
bool converged = false; bool converged = false;
if (config.vio_use_lm) { // Use LevenbergMarquardt
bool step = false; bool step = false;
int max_iter = 10; int max_iter = 10;
@ -1005,11 +1008,13 @@ void KeypointVioEstimator::optimize() {
double f_diff = (error_total - after_error_total); double f_diff = (error_total - after_error_total);
double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB()); double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl; // std::cout << "f_diff " << f_diff << " l_diff " << l_diff <<
// std::endl;
double step_quality = f_diff / l_diff; double step_quality = f_diff / l_diff;
if (step_quality < 0) { if (step_quality < 0) {
if (config.vio_debug)
std::cout << "\t[REJECTED] lambda:" << lambda std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality << " step_quality: " << step_quality
<< " max_inc: " << max_inc << " max_inc: " << max_inc
@ -1019,6 +1024,7 @@ void KeypointVioEstimator::optimize() {
restore(); restore();
} else { } else {
if (config.vio_debug)
std::cout << "\t[ACCEPTED] lambda:" << lambda std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality << " step_quality: " << step_quality
<< " max_inc: " << max_inc << " max_inc: " << max_inc
@ -1035,9 +1041,40 @@ void KeypointVioEstimator::optimize() {
max_iter--; max_iter--;
} }
if (converged) { if (config.vio_debug && converged) {
std::cout << "[CONVERGED]" << std::endl; 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);
const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-4) converged = true;
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
kv.second.applyInc(-inc.segment<POSE_SIZE>(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<POSE_VEL_BIAS_SIZE>(idx));
}
// Update points
tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
auto update_points_func = [&](const tbb::blocked_range<size_t>& 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);
}
if (config.vio_debug) { if (config.vio_debug) {
double after_update_marg_prior_error = 0; double after_update_marg_prior_error = 0;
@ -1099,7 +1136,7 @@ void KeypointVioEstimator::optimize() {
if (config.vio_debug) { if (config.vio_debug) {
std::cout << "=================================" << std::endl; std::cout << "=================================" << std::endl;
} }
} } // namespace basalt
void KeypointVioEstimator::computeProjections( void KeypointVioEstimator::computeProjections(
std::vector<Eigen::vector<Eigen::Vector4d>>& data) const { std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {