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

Make default config suitable for float. Implement better handling of non-finite increments.

This commit is contained in:
Nikolaus Demmel 2021-12-10 11:03:49 +01:00
parent ec48ff22e5
commit 700376d846
6 changed files with 54 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/utils/vio_config.h
View File

@ -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;

2
include/basalt/vi_estimator/sqrt_keypoint_vio.h
View File

@ -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<std::thread> processing_thread;

2
include/basalt/vi_estimator/sqrt_keypoint_vo.h
View File

@ -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<std::thread> processing_thread;

8
src/utils/vio_config.cpp
View File

@ -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));

41
src/vi_estimator/sqrt_keypoint_vio.cpp
View File

@ -1202,21 +1202,32 @@ void SqrtKeypointVioEstimator<Scalar_>::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<Eigen::Ref<MatX>> 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<Eigen::Ref<MatX>> 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<Scalar_>::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<Scalar_>::optimize() {
1 - std::pow<Scalar>(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<Scalar_>::optimize() {
}
lambda = lambda_vee * lambda;
constexpr Scalar vee_factor = Scalar(2.0);
lambda_vee *= vee_factor;
// lambda = std::max(min_lambda, lambda);

37
src/vi_estimator/sqrt_keypoint_vo.cpp
View File

@ -1099,21 +1099,32 @@ void SqrtKeypointVoEstimator<Scalar_>::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<Eigen::Ref<MatX>> 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<Eigen::Ref<MatX>> 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<Scalar_>::optimize() {
1 - std::pow<Scalar>(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<Scalar_>::optimize() {
}
lambda = lambda_vee * lambda;
constexpr Scalar vee_factor = Scalar(2.0);
lambda_vee *= vee_factor;
// lambda = std::max(min_lambda, lambda);