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

Better assert when we receive out-of-order image frames

This commit is contained in:
Moses Turner 2022-09-18 10:42:51 -05:00
parent d2db20106d
commit debea94103
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/vi_estimator/sqrt_keypoint_vio.cpp
View File

@ -230,10 +230,14 @@ 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, BASALT_ASSERT_MSG(prev_frame->t_ns != curr_frame->t_ns,
"duplicate frame timestamps?! zero time delta leads " "duplicate frame timestamps?! zero time delta leads "
"to invalid IMU integration."); "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) { while (data->t_ns <= prev_frame->t_ns) {
data = popFromImuDataQueue(); data = popFromImuDataQueue();
if (!data) break; if (!data) break;