From ec48ff22e512e83b861a5d45b6e8e12c4e935fa1 Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Fri, 10 Dec 2021 02:27:35 +0100 Subject: [PATCH] vo: crash earlier for duplicate timestamps or numeric failure --- include/basalt/utils/vio_config.h | 2 +- src/utils/vio_config.cpp | 2 +- src/vi_estimator/sqrt_keypoint_vio.cpp | 12 ++++++++++++ src/vi_estimator/sqrt_keypoint_vo.cpp | 8 ++++++++ 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index 87b1baf..07d39ef 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -78,7 +78,7 @@ struct VioConfig { double vio_lm_lambda_initial; double vio_lm_lambda_min; double vio_lm_lambda_max; - int vio_lm_landmark_damping_variant; + int vio_lm_landmark_damping_variant; // currently unused int vio_lm_pose_damping_variant; bool vio_scale_jacobian; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 4e39f70..9491cc7 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -78,7 +78,7 @@ VioConfig::VioConfig() { vio_lm_lambda_initial = 1e-8; vio_lm_lambda_min = 1e-32; vio_lm_lambda_max = 1e2; - vio_lm_landmark_damping_variant = 0; + vio_lm_landmark_damping_variant = 0; // currently unused vio_lm_pose_damping_variant = 0; vio_scale_jacobian = true; diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index 71aff49..4cbc6cc 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -226,6 +226,10 @@ void SqrtKeypointVioEstimator::initialize(const Eigen::Vector3d& bg_, prev_frame->t_ns, last_state.getState().bias_gyro, last_state.getState().bias_accel)); + BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns, + "duplicate frame timestamps?! zero time delta leads " + "to invalid IMU integration."); + while (data->t_ns <= prev_frame->t_ns) { data = popFromImuDataQueue(); if (!data) break; @@ -1127,6 +1131,7 @@ void SqrtKeypointVioEstimator::optimize() { } if (config.vio_debug) { + // TODO: num_points debug output missing std::cout << "[LINEARIZE] Error: " << error_total << " num points " << std::endl; std::cout << "Iteration " << it << " " << error_total << std::endl; @@ -1205,6 +1210,13 @@ void SqrtKeypointVioEstimator::optimize() { 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"); } // backup state (then apply increment and check cost decrease) diff --git a/src/vi_estimator/sqrt_keypoint_vo.cpp b/src/vi_estimator/sqrt_keypoint_vo.cpp index 92838dd..e7fd066 100644 --- a/src/vi_estimator/sqrt_keypoint_vo.cpp +++ b/src/vi_estimator/sqrt_keypoint_vo.cpp @@ -1029,6 +1029,7 @@ void SqrtKeypointVoEstimator::optimize() { stats.add("performQR", t.reset()).format("ms"); if (config.vio_debug) { + // TODO: num_points debug output missing std::cout << "[LINEARIZE] Error: " << error_total << " num points " << std::endl; std::cout << "Iteration " << it << " " << error_total << std::endl; @@ -1106,6 +1107,13 @@ void SqrtKeypointVoEstimator::optimize() { 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"); } // backup state (then apply increment and check cost decrease)