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

small fixes LM

This commit is contained in:
Vladyslav Usenko 2019-08-14 10:57:48 +02:00
parent 397ddc159c
commit 77142f0c2a
2 changed files with 9 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/vi_estimator/keypoint_vio.cpp
View File

@ -1014,18 +1014,11 @@ void KeypointVioEstimator::optimize() {
after_update_marg_prior_error + after_bg_error + after_ba_error; after_update_marg_prior_error + after_bg_error + after_ba_error;
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());
// std::cout << "f_diff " << f_diff << " l_diff " << l_diff << if (f_diff < 0) {
// std::endl;
double step_quality = f_diff / l_diff;
if (step_quality < 0) {
if (config.vio_debug) if (config.vio_debug)
std::cout << "\t[REJECTED] lambda:" << lambda std::cout << "\t[REJECTED] lambda:" << lambda
<< " step_quality: " << step_quality << " f_diff: " << f_diff << " max_inc: " << max_inc
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl; << " Error: " << after_error_total << std::endl;
lambda = std::min(max_lambda, lambda_vee * lambda); lambda = std::min(max_lambda, lambda_vee * lambda);
lambda_vee *= 2; lambda_vee *= 2;
@ -1034,14 +1027,10 @@ void KeypointVioEstimator::optimize() {
} else { } else {
if (config.vio_debug) if (config.vio_debug)
std::cout << "\t[ACCEPTED] lambda:" << lambda std::cout << "\t[ACCEPTED] lambda:" << lambda
<< " step_quality: " << step_quality << " f_diff: " << f_diff << " max_inc: " << max_inc
<< " max_inc: " << max_inc
<< " Error: " << after_error_total << std::endl; << " Error: " << after_error_total << std::endl;
lambda = std::max( lambda = std::max(min_lambda, lambda / 3);
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2; lambda_vee = 2;
step = true; step = true;

20
src/vi_estimator/nfr_mapper.cpp
View File

@ -292,7 +292,7 @@ void NfrMapper::optimize(int num_iterations) {
bool converged = false; bool converged = false;
if (config.vio_use_lm) { // Use Levenberg–Marquardt if (config.mapper_use_lm) { // Use Levenberg–Marquardt
bool step = false; bool step = false;
int max_iter = 10; int max_iter = 10;
@ -336,15 +336,9 @@ void NfrMapper::optimize(int num_iterations) {
after_roll_pitch_error; after_roll_pitch_error;
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());
std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl; if (f_diff < 0) {
std::cout << "\t[REJECTED] lambda:" << lambda << " f_diff: " << f_diff
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 << " max_inc: " << max_inc
<< " vision_error: " << after_update_vision_error << " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error << " rel_error: " << after_rel_error
@ -355,18 +349,14 @@ void NfrMapper::optimize(int num_iterations) {
restore(); restore();
} else { } else {
std::cout << "\t[ACCEPTED] lambda:" << lambda std::cout << "\t[ACCEPTED] lambda:" << lambda << " f_diff: " << f_diff
<< " step_quality: " << step_quality
<< " max_inc: " << max_inc << " max_inc: " << max_inc
<< " vision_error: " << after_update_vision_error << " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error << " rel_error: " << after_rel_error
<< " roll_pitch_error: " << after_roll_pitch_error << " roll_pitch_error: " << after_roll_pitch_error
<< " total: " << after_error_total << std::endl; << " total: " << after_error_total << std::endl;
lambda = std::max( lambda = std::max(min_lambda, lambda / 3);
min_lambda,
lambda *
std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
lambda_vee = 2; lambda_vee = 2;
step = true; step = true;