From debea941034fa96475c16d6f5e281b17decb76bd Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Sun, 18 Sep 2022 10:42:51 -0500 Subject: [PATCH] Better assert when we receive out-of-order image frames --- src/vi_estimator/sqrt_keypoint_vio.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index bdbcdb7..cf4f16e 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -230,10 +230,14 @@ 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, + BASALT_ASSERT_MSG(prev_frame->t_ns != curr_frame->t_ns, "duplicate frame timestamps?! zero time delta leads " "to invalid IMU integration."); + BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns, + "frame timestamps not monotonically increasing?! " + "are we going 88mph and inside a delorean?"); + while (data->t_ns <= prev_frame->t_ns) { data = popFromImuDataQueue(); if (!data) break;