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,11 +302,13 @@ class SplineOptimization {
// std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl; // std::cout << "spline.minTimeNs() " << spline.minTimeNs() << std::endl;
// std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl; // std::cout << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
while (mocap_measurements.front().timestamp_ns <= while (!mocap_measurements.empty() &&
mocap_measurements.front().timestamp_ns <=
spline.minTimeNs() + spline.getDtNs()) spline.minTimeNs() + spline.getDtNs())
mocap_measurements.pop_front(); mocap_measurements.pop_front();
while (mocap_measurements.back().timestamp_ns >= while (!mocap_measurements.empty() &&
mocap_measurements.back().timestamp_ns >=
spline.maxTimeNs() - spline.getDtNs()) spline.maxTimeNs() - spline.getDtNs())
mocap_measurements.pop_back(); mocap_measurements.pop_back();