The open source OpenXR runtime
t/slam: Turn timestamp asserts into warnings
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
1da029ad
f8932fe8
+19
-12
+19
-12
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+19
-12
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
1315
1315
xrt_vec3_f64 a = s->accel_m_s2;
1316
1316
xrt_vec3_f64 w = s->gyro_rad_secs;
1317
1317
1318
+
timepoint_ns now = (timepoint_ns)os_monotonic_get_ns();
1319
+
SLAM_TRACE("[%ld] imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", now, ts, a.x, a.y, a.z, w.x, w.y, w.z);
1320
+
// Check monotonically increasing timestamps
1321
+
if (ts <= t.last_imu_ts) {
1322
+
SLAM_WARN("Sample (%ld) is older than last (%ld)", ts, t.last_imu_ts);
1323
+
return;
1324
+
}
1325
+
t.last_imu_ts = ts;
1326
+
1318
1327
//! @todo There are many conversions like these between xrt and
1319
1328
//! slam_tracker.hpp types. Implement a casting mechanism to avoid copies.
1320
1329
imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z};
1321
1330
if (t.submit) {
1322
1331
t.slam->push_imu_sample(sample);
1323
1332
}
1324
-
SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", ts, a.x, a.y, a.z, w.x, w.y, w.z);
1325
-
1326
-
// Check monotonically increasing timestamps
1327
-
SLAM_DASSERT(ts > t.last_imu_ts, "Sample (%ld) is older than last (%ld)", ts, t.last_imu_ts);
1328
-
t.last_imu_ts = ts;
1329
1333
1330
1334
xrt_sink_push_imu(t.euroc_recorder->imu, s);
1331
1335
···
1348
1352
}
1349
1353
SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame");
1350
1354
1355
+
// Check monotonically increasing timestamps
1356
+
timepoint_ns &last_ts = t.last_cam_ts[cam_index];
1357
+
timepoint_ns ts = (int64_t)frame->timestamp;
1358
+
SLAM_TRACE("[%ld] cam%d frame t=%ld", os_monotonic_get_ns(), cam_index, ts);
1359
+
if (last_ts >= ts) {
1360
+
SLAM_WARN("Frame (%ld) is older than last (%ld)", ts, last_ts);
1361
+
}
1362
+
last_ts = ts;
1363
+
1351
1364
// Construct and send the image sample
1352
1365
cv::Mat img = t.cv_wrapper->wrap(frame);
1353
1366
SLAM_DASSERT_(frame->timestamp < INT64_MAX);
1354
-
img_sample sample{(int64_t)frame->timestamp, img, cam_index};
1367
+
img_sample sample{ts, img, cam_index};
1355
1368
if (t.submit) {
1356
1369
XRT_TRACE_IDENT(slam_push);
1357
1370
t.slam->push_frame(sample);
1358
1371
}
1359
-
SLAM_TRACE("cam%d frame t=%lu", cam_index, frame->timestamp);
1360
-
1361
-
// Check monotonically increasing timestamps
1362
-
timepoint_ns &last_ts = t.last_cam_ts[cam_index];
1363
-
SLAM_DASSERT(sample.timestamp > last_ts, "Frame (%ld) is older than last (%ld)", sample.timestamp, last_ts);
1364
-
last_ts = sample.timestamp;
1365
1372
}
1366
1373
1367
1374
#define DEFINE_RECEIVE_CAM(cam_id) \