29 : maxTimeInterpolation(std::chrono::seconds(-1))
44 m_path[t] = p.asTPose();
60 bool& out_valid_interp)
const
72 bool& out_valid_interp)
const
75 for (
size_t k = 0; k < pose_t::static_size; k++)
80 p1.second = p2.second = p3.second = p4.second = out_interp;
85 bool interp_method_requires_4pts;
91 interp_method_requires_4pts =
false;
94 interp_method_requires_4pts =
true;
99 auto it_ge1 = m_path.lower_bound(t);
102 if (it_ge1 != m_path.end() && it_ge1->first == t)
104 out_interp = it_ge1->second;
105 out_valid_interp =
true;
110 if (it_ge1 == m_path.end() || it_ge1 == m_path.begin())
112 out_valid_interp =
false;
117 auto it_ge2 = it_ge1;
119 if (it_ge2 == m_path.end())
121 if (interp_method_requires_4pts)
123 out_valid_interp =
false;
134 if (it_ge1 == m_path.begin())
136 if (interp_method_requires_4pts)
138 out_valid_interp =
false;
150 ? (p2.first - p1.first)
154 ? (p4.first - p3.first)
157 if (maxTimeInterpolation.count() > 0 &&
158 (dt12 > maxTimeInterpolation || dt23 > maxTimeInterpolation ||
159 dt34 > maxTimeInterpolation))
161 out_valid_interp =
false;
173 impl_interpolation(p1, p2, p3, p4, m_method, t, out_interp);
175 out_valid_interp =
true;
185 bool ret = getPreviousPoseWithMinDistance(t,
distance, p);
194 if (m_path.size() == 0 ||
distance <= 0)
return false;
199 auto it = m_path.
find(t);
200 if (it != m_path.end() && it != m_path.begin())
210 }
while (d <
distance && it != m_path.begin());
214 out_pose = it->second;
226 maxTimeInterpolation = time;
232 return maxTimeInterpolation;
242 if (!f.is_open())
return false;
244 for (
auto i = m_path.begin(); i != m_path.end(); ++i)
247 const auto& p = i->second;
250 for (
unsigned int k = 0; k < p.size(); k++)
252 str += std::string(
"\n");
273 if (!f.is_open())
return false;
274 if (m_path.empty())
return true;
286 if (!valid)
continue;
289 for (
unsigned int k = 0; k < p.size(); k++)
291 str += std::string(
"\n");
314 catch (std::exception&)
320 if (M.
rows() == 0)
return false;
324 const size_t N = M.
rows();
326 for (
size_t i = 0; i < N; i++)
328 for (
unsigned int k = 0; k < pose_t::static_size; k++)
343 for (
unsigned int k = 0; k < point_t::static_size; k++)
345 Min[k] = std::numeric_limits<double>::max();
346 Max[k] = -std::numeric_limits<double>::max();
349 for (
auto p = m_path.begin(); p != m_path.end(); ++p)
351 for (
unsigned int k = 0; k < point_t::static_size; k++)
375 unsigned int component,
unsigned int samples)
377 if (m_path.empty())
return;
382 size_t nitems =
size();
384 post = (samples % 2) ? (
unsigned int)(samples / 2) : samples / 2;
385 ant = (
unsigned int)(samples / 2);
391 for (it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k)
393 it2 = m_path.begin();
394 if (k - ant > 0) advance(it2, k - ant);
396 if (k + post < (
int)nitems)
398 it3 = m_path.begin();
399 advance(it3, k + post + 1);
406 unsigned int nsamples =
distance(it2, it3);
408 for (
unsigned int i = 0; it2 != it3; ++it2, ++i)