This commit is contained in:
Vladyslav Usenko 2019-07-10 11:58:11 +02:00
parent 3891040307
commit 9c5f5f3394
1 changed files with 6 additions and 4 deletions

View File

@ -302,12 +302,14 @@ class SplineOptimization {
// std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl;
// std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
while (mocap_measurements.front().timestamp_ns <=
spline.minTimeNs() + spline.getDtNs())
while (!mocap_measurements.empty() &&
mocap_measurements.front().timestamp_ns <=
spline.minTimeNs() + spline.getDtNs())
mocap_measurements.pop_front();
while (mocap_measurements.back().timestamp_ns >=
spline.maxTimeNs() - spline.getDtNs())
while (!mocap_measurements.empty() &&
mocap_measurements.back().timestamp_ns >=
spline.maxTimeNs() - spline.getDtNs())
mocap_measurements.pop_back();
ccd.calibration = calib.get();