diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 07d39ef..cebf67f 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -78,8 +78,6 @@ struct VioConfig { double vio_lm_lambda_initial; double vio_lm_lambda_min; double vio_lm_lambda_max; - int vio_lm_landmark_damping_variant; // currently unused - int vio_lm_pose_damping_variant; bool vio_scale_jacobian; diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vio.h b/include/basalt/vi_estimator/sqrt_keypoint_vio.h index 376ef10..69494db 100644 --- a/include/basalt/vi_estimator/sqrt_keypoint_vio.h +++ b/include/basalt/vi_estimator/sqrt_keypoint_vio.h @@ -248,6 +248,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase, VioConfig config; + constexpr static Scalar vee_factor = Scalar(2.0); + constexpr static Scalar initial_vee = Scalar(2.0); Scalar lambda, min_lambda, max_lambda, lambda_vee; std::shared_ptr processing_thread; diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vo.h b/include/basalt/vi_estimator/sqrt_keypoint_vo.h index 555fb14..b20124b 100644 --- a/include/basalt/vi_estimator/sqrt_keypoint_vo.h +++ b/include/basalt/vi_estimator/sqrt_keypoint_vo.h @@ -238,6 +238,8 @@ class SqrtKeypointVoEstimator : public VioEstimatorBase, VioConfig config; + constexpr static Scalar vee_factor = Scalar(2.0); + constexpr static Scalar initial_vee = Scalar(2.0); Scalar lambda, min_lambda, max_lambda, lambda_vee; std::shared_ptr processing_thread; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 9491cc7..0e554ed 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -75,11 +75,9 @@ VioConfig::VioConfig() { vio_enforce_realtime = false; vio_use_lm = false; - vio_lm_lambda_initial = 1e-8; - vio_lm_lambda_min = 1e-32; + vio_lm_lambda_initial = 1e-4; + vio_lm_lambda_min = 1e-6; vio_lm_lambda_max = 1e2; - vio_lm_landmark_damping_variant = 0; // currently unused - vio_lm_pose_damping_variant = 0; vio_scale_jacobian = true; @@ -192,8 +190,6 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.vio_lm_lambda_initial)); ar(CEREAL_NVP(config.vio_lm_lambda_min)); ar(CEREAL_NVP(config.vio_lm_lambda_max)); - ar(CEREAL_NVP(config.vio_lm_landmark_damping_variant)); - ar(CEREAL_NVP(config.vio_lm_pose_damping_variant)); ar(CEREAL_NVP(config.vio_scale_jacobian)); diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index 4cbc6cc..55a7618 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -1202,21 +1202,32 @@ void SqrtKeypointVioEstimator::optimize() { stats.add("get_dense_H_b", t.reset()).format("ms"); - if (config.vio_lm_pose_damping_variant == 1) { + int iter = 0; + bool inc_valid = false; + constexpr int max_num_iter = 3; + + while (iter < max_num_iter && !inc_valid) { VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda); - H.diagonal() += Hdiag_lambda; + MatX H_copy = H; + H_copy.diagonal() += Hdiag_lambda; + + Eigen::LDLT> ldlt(H_copy); + inc = ldlt.solve(b); + stats.add("solve", t.reset()).format("ms"); + + if (!inc.array().isFinite().all()) { + lambda = lambda_vee * lambda; + lambda_vee *= vee_factor; + } else { + inc_valid = true; + } + iter++; } - Eigen::LDLT> ldlt(H); - inc = ldlt.solve(b); - stats.add("solve", t.reset()).format("ms"); - - // TODO: instead of crashing, backtrack and increase damping, but make - // sure it does not go unnoticed. (Note: right now, without further - // handling, Sophus would crash anyway when trying to apply and - // increment with NaNs or inf) - BASALT_ASSERT_MSG(!inc.array().isFinite().all(), - "numeric failure during"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1295,8 +1306,8 @@ void SqrtKeypointVioEstimator::optimize() { relative_decrease, step_norminf); } - // TODO: consider to remove assert. For now we want to test if we even - // run into the l_diff <= 0 case ever in practice + // TODO: consider to remove assert. For now we want to test if we + // even run into the l_diff <= 0 case ever in practice // BASALT_ASSERT_STREAM(l_diff > 0, "l_diff " << l_diff); // l_diff <= 0 is a theoretical possibility if the model cost change @@ -1339,7 +1350,6 @@ void SqrtKeypointVioEstimator::optimize() { 1 - std::pow(2 * relative_decrease - 1, 3)); lambda = std::max(min_lambda, lambda); - constexpr Scalar initial_vee = Scalar(2.0); lambda_vee = initial_vee; it++; @@ -1368,7 +1378,6 @@ void SqrtKeypointVioEstimator::optimize() { } lambda = lambda_vee * lambda; - constexpr Scalar vee_factor = Scalar(2.0); lambda_vee *= vee_factor; // lambda = std::max(min_lambda, lambda); diff --git a/src/vi_estimator/sqrt_keypoint_vo.cpp b/src/vi_estimator/sqrt_keypoint_vo.cpp index e7fd066..968df71 100644 --- a/src/vi_estimator/sqrt_keypoint_vo.cpp +++ b/src/vi_estimator/sqrt_keypoint_vo.cpp @@ -1099,21 +1099,32 @@ void SqrtKeypointVoEstimator::optimize() { stats.add("get_dense_H_b", t.reset()).format("ms"); - if (config.vio_lm_pose_damping_variant == 1) { + int iter = 0; + bool inc_valid = false; + constexpr int max_num_iter = 3; + + while (iter < max_num_iter && !inc_valid) { VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda); - H.diagonal() += Hdiag_lambda; + MatX H_copy = H; + H_copy.diagonal() += Hdiag_lambda; + + Eigen::LDLT> ldlt(H_copy); + inc = ldlt.solve(b); + stats.add("solve", t.reset()).format("ms"); + + if (!inc.array().isFinite().all()) { + lambda = lambda_vee * lambda; + lambda_vee *= vee_factor; + } else { + inc_valid = true; + } + iter++; } - Eigen::LDLT> ldlt(H); - inc = ldlt.solve(b); - stats.add("solve", t.reset()).format("ms"); - - // TODO: instead of crashing, backtrack and increase damping, but make - // sure it does not go unnoticed. (Note: right now, without further - // handling, Sophus would crash anyway when trying to apply and - // increment with NaNs or inf) - BASALT_ASSERT_MSG(!inc.array().isFinite().all(), - "numeric failure during"); + if (!inc_valid) { + std::cerr << "Still invalid inc after " << max_num_iter + << " iterations." << std::endl; + } } // backup state (then apply increment and check cost decrease) @@ -1225,7 +1236,6 @@ void SqrtKeypointVoEstimator::optimize() { 1 - std::pow(2 * relative_decrease - 1, 3)); lambda = std::max(min_lambda, lambda); - constexpr Scalar initial_vee = Scalar(2.0); lambda_vee = initial_vee; it++; @@ -1253,7 +1263,6 @@ void SqrtKeypointVoEstimator::optimize() { } lambda = lambda_vee * lambda; - constexpr Scalar vee_factor = Scalar(2.0); lambda_vee *= vee_factor; // lambda = std::max(min_lambda, lambda);