vo: crash earlier for duplicate timestamps or numeric failure

This commit is contained in:
Nikolaus Demmel 2021-12-10 02:27:35 +01:00
parent 470ac63a2d
commit ec48ff22e5
4 changed files with 22 additions and 2 deletions

View File

@ -78,7 +78,7 @@ struct VioConfig {
double vio_lm_lambda_initial; double vio_lm_lambda_initial;
double vio_lm_lambda_min; double vio_lm_lambda_min;
double vio_lm_lambda_max; 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; int vio_lm_pose_damping_variant;
bool vio_scale_jacobian; bool vio_scale_jacobian;

View File

@ -78,7 +78,7 @@ VioConfig::VioConfig() {
vio_lm_lambda_initial = 1e-8; vio_lm_lambda_initial = 1e-8;
vio_lm_lambda_min = 1e-32; vio_lm_lambda_min = 1e-32;
vio_lm_lambda_max = 1e2; 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_lm_pose_damping_variant = 0;
vio_scale_jacobian = true; vio_scale_jacobian = true;

View File

@ -226,6 +226,10 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
prev_frame->t_ns, last_state.getState().bias_gyro, prev_frame->t_ns, last_state.getState().bias_gyro,
last_state.getState().bias_accel)); 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) { while (data->t_ns <= prev_frame->t_ns) {
data = popFromImuDataQueue(); data = popFromImuDataQueue();
if (!data) break; if (!data) break;
@ -1127,6 +1131,7 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
} }
if (config.vio_debug) { if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points " std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl; << std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl; std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1205,6 +1210,13 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H); Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
inc = ldlt.solve(b); inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms"); 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) // backup state (then apply increment and check cost decrease)

View File

@ -1029,6 +1029,7 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
stats.add("performQR", t.reset()).format("ms"); stats.add("performQR", t.reset()).format("ms");
if (config.vio_debug) { if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points " std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl; << std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl; std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1106,6 +1107,13 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H); Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
inc = ldlt.solve(b); inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms"); 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) // backup state (then apply increment and check cost decrease)