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

update mapper

This commit is contained in:
Vladyslav Usenko 2019-08-12 19:24:25 +02:00
parent aaa82fba3c
commit 7ebb7baf7c
1 changed files with 91 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
src/vi_estimator/nfr_mapper.cpp
View File

@ -45,9 +45,9 @@ namespace basalt {
NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
: config(config),
lambda(1e-10),
min_lambda(1e-32),
max_lambda(1000),
lambda(config.mapper_lm_lambda_min),
min_lambda(config.mapper_lm_lambda_min),
max_lambda(config.mapper_lm_lambda_max),
lambda_vee(2) {
this->calib = calib;
this->obs_std_dev = config.mapper_obs_std_dev;
@ -291,11 +291,95 @@ void NfrMapper::optimize(int num_iterations) {
const 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-5) converged = true;
backup();
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
BASALT_ASSERT(!kv.second.isLinearized());
kv.second.applyInc(-inc.segment<POSE_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);
double after_update_vision_error = 0;
double after_rel_error = 0;
double after_roll_pitch_error = 0;
computeError(after_update_vision_error);
computeRelPose(after_rel_error);
computeRollPitch(after_roll_pitch_error);
double after_error_total = after_update_vision_error + after_rel_error +
after_roll_pitch_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
<< " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error
<< " roll_pitch_error: " << after_roll_pitch_error
<< " total: " << 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
<< " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error
<< " roll_pitch_error: " << after_roll_pitch_error
<< " total: " << 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 (after_error_total > error_total) {
std::cout << "increased error after update!!!" << 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);
@ -303,8 +387,6 @@ void NfrMapper::optimize(int num_iterations) {
double max_inc = inc.array().abs().maxCoeff();
if (max_inc < 1e-5) converged = true;
backup();
// apply increment to poses
for (auto& kv : frame_poses) {
int idx = aom.abs_order_map.at(kv.first).first;
@ -321,60 +403,6 @@ void NfrMapper::optimize(int num_iterations) {
}
};
tbb::parallel_for(keys_range, update_points_func);
double after_update_vision_error = 0;
double after_rel_error = 0;
double after_roll_pitch_error = 0;
computeError(after_update_vision_error);
computeRelPose(after_rel_error);
computeRollPitch(after_roll_pitch_error);
double after_error_total =
after_update_vision_error + after_rel_error + after_roll_pitch_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
<< " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error
<< " roll_pitch_error: " << after_roll_pitch_error
<< " total: " << 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
<< " vision_error: " << after_update_vision_error
<< " rel_error: " << after_rel_error
<< " roll_pitch_error: " << after_roll_pitch_error
<< " total: " << 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 (after_error_total > error_total) {
std::cout << "increased error after update!!!" << std::endl;
}
}
auto t2 = std::chrono::high_resolution_clock::now();