diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 949119d..b0c8c44 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -45,9 +45,9 @@ namespace basalt { NfrMapper::NfrMapper(const Calibration& 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(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_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();