MRPT  1.9.9
CPoseInterpolatorBase.hpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in https://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+
9  */
10 
12 
13 #include <mrpt/math/CMatrixD.h>
14 //#include <mrpt/math/eigen_extensions.h>
15 #include <mrpt/math/slerp.h>
16 #include <mrpt/math/wrap2pi.h>
17 #include <mrpt/poses/CPose2D.h>
18 #include <mrpt/poses/CPose3D.h>
21 #include <mrpt/system/datetime.h>
22 #include <fstream>
23 #include <mrpt/math/interp_fit.hpp>
24 
25 namespace mrpt::poses
26 {
27 template <int DIM>
29  : maxTimeInterpolation(std::chrono::seconds(-1))
30 
31 {
32 }
33 
34 template <int DIM>
36 {
37  m_path.clear();
38 }
39 
40 template <int DIM>
42  const mrpt::Clock::time_point& t, const cpose_t& p)
43 {
44  m_path[t] = p.asTPose();
45 }
46 template <int DIM>
48  const mrpt::Clock::time_point& t, const pose_t& p)
49 {
50  m_path[t] = p;
51 }
52 
53 /*---------------------------------------------------------------
54  interpolate
55  ---------------------------------------------------------------*/
56 template <int DIM>
59  const mrpt::Clock::time_point& t, cpose_t& out_interp,
60  bool& out_valid_interp) const
61 {
62  pose_t p;
63  this->interpolate(t, p, out_valid_interp);
64  out_interp = cpose_t(p);
65  return out_interp;
66 }
67 
68 template <int DIM>
71  const mrpt::Clock::time_point& t, pose_t& out_interp,
72  bool& out_valid_interp) const
73 {
74  // Default value in case of invalid interp
75  for (size_t k = 0; k < pose_t::static_size; k++)
76  {
77  out_interp[k] = 0;
78  }
79  TTimePosePair p1, p2, p3, p4;
80  p1.second = p2.second = p3.second = p4.second = out_interp;
81 
82  // We'll look for 4 consecutive time points.
83  // Check if the selected method needs all 4 points or just the central 2 of
84  // them:
85  bool interp_method_requires_4pts;
86  switch (m_method)
87  {
88  case imLinear2Neig:
89  case imSplineSlerp:
90  case imLinearSlerp:
91  interp_method_requires_4pts = false;
92  break;
93  default:
94  interp_method_requires_4pts = true;
95  break;
96  };
97 
98  // Out of range?
99  auto it_ge1 = m_path.lower_bound(t);
100 
101  // Exact match?
102  if (it_ge1 != m_path.end() && it_ge1->first == t)
103  {
104  out_interp = it_ge1->second;
105  out_valid_interp = true;
106  return out_interp;
107  }
108 
109  // Are we in the beginning or the end of the path?
110  if (it_ge1 == m_path.end() || it_ge1 == m_path.begin())
111  {
112  out_valid_interp = false;
113  return out_interp;
114  } // end
115 
116  p3 = *it_ge1; // Third pair
117  auto it_ge2 = it_ge1;
118  ++it_ge2;
119  if (it_ge2 == m_path.end())
120  {
121  if (interp_method_requires_4pts)
122  {
123  out_valid_interp = false;
124  return out_interp;
125  }
126  }
127  else
128  {
129  p4 = *(it_ge2); // Fourth pair
130  }
131 
132  p2 = *(--it_ge1); // Second pair
133 
134  if (it_ge1 == m_path.begin())
135  {
136  if (interp_method_requires_4pts)
137  {
138  out_valid_interp = false;
139  return out_interp;
140  }
141  }
142  else
143  {
144  p1 = *(--it_ge1); // First pair
145  }
146 
147  // Test if the difference between the desired timestamp and the next
148  // timestamp is lower than a certain (configurable) value
149  const mrpt::Clock::duration dt12 = interp_method_requires_4pts
150  ? (p2.first - p1.first)
152  const mrpt::Clock::duration dt23 = (p3.first - p2.first);
153  const mrpt::Clock::duration dt34 = interp_method_requires_4pts
154  ? (p4.first - p3.first)
156 
157  if (maxTimeInterpolation.count() > 0 &&
158  (dt12 > maxTimeInterpolation || dt23 > maxTimeInterpolation ||
159  dt34 > maxTimeInterpolation))
160  {
161  out_valid_interp = false;
162  return out_interp;
163  }
164 
165  // Do interpolation:
166  // ------------------------------------------
167  // First Previous point: p1
168  // Second Previous point: p2
169  // First Next point: p3
170  // Second Next point: p4
171  // Time where to interpolate: t
172 
173  impl_interpolation(p1, p2, p3, p4, m_method, t, out_interp);
174 
175  out_valid_interp = true;
176  return out_interp;
177 
178 } // end interpolate
179 
180 template <int DIM>
182  const mrpt::Clock::time_point& t, double distance, cpose_t& out_pose)
183 {
184  pose_t p;
185  bool ret = getPreviousPoseWithMinDistance(t, distance, p);
186  out_pose = cpose_t(p);
187  return ret;
188 }
189 
190 template <int DIM>
192  const mrpt::Clock::time_point& t, double distance, pose_t& out_pose)
193 {
194  if (m_path.size() == 0 || distance <= 0) return false;
195 
196  pose_t myPose;
197 
198  // Search for the desired timestamp
199  auto it = m_path.find(t);
200  if (it != m_path.end() && it != m_path.begin())
201  myPose = it->second;
202  else
203  return false;
204 
205  double d = 0.0;
206  do
207  {
208  --it;
209  d = (point_t(myPose) - point_t(it->second)).norm();
210  } while (d < distance && it != m_path.begin());
211 
212  if (d >= distance)
213  {
214  out_pose = it->second;
215  return true;
216  }
217  else
218  return false;
219 } // end getPreviousPose
220 
221 template <int DIM>
223  const mrpt::Clock::duration& time)
224 {
225  ASSERT_(time.count() > 0);
226  maxTimeInterpolation = time;
227 }
228 
229 template <int DIM>
231 {
232  return maxTimeInterpolation;
233 }
234 
235 template <int DIM>
237 {
238  try
239  {
240  std::ofstream f;
241  f.open(s);
242  if (!f.is_open()) return false;
243  std::string str;
244  for (auto i = m_path.begin(); i != m_path.end(); ++i)
245  {
246  const double t = mrpt::system::timestampTotime_t(i->first);
247  const auto& p = i->second;
248 
249  str = mrpt::format("%.06f ", t);
250  for (unsigned int k = 0; k < p.size(); k++)
251  str += mrpt::format("%.06f ", p[k]);
252  str += std::string("\n");
253 
254  f << str;
255  }
256  return true;
257  }
258  catch (...)
259  {
260  return false;
261  }
262 }
263 
264 template <int DIM>
266  const std::string& s, const mrpt::Clock::duration& period) const
267 {
268  using mrpt::Clock;
269  try
270  {
271  std::ofstream f;
272  f.open(s);
273  if (!f.is_open()) return false;
274  if (m_path.empty()) return true;
275 
276  std::string str;
277 
278  const Clock::time_point t_ini = m_path.begin()->first;
279  const Clock::time_point t_end = m_path.rbegin()->first;
280 
281  pose_t p;
282  bool valid;
283  for (Clock::time_point t = t_ini; t <= t_end; t += period)
284  {
285  this->interpolate(t, p, valid);
286  if (!valid) continue;
287 
289  for (unsigned int k = 0; k < p.size(); k++)
290  str += mrpt::format("%.06f ", p[k]);
291  str += std::string("\n");
292  f << str;
293  }
294  return true;
295  }
296  catch (...)
297  {
298  return false;
299  }
300 }
301 
302 template <int DIM>
304 {
305  MRPT_START
306 
307  clear();
309 
310  try
311  {
312  M.loadFromTextFile(s);
313  }
314  catch (std::exception&)
315  {
316  return false; // error loading file
317  }
318 
319  // Check valid format:
320  if (M.rows() == 0) return false;
321  ASSERT_(M.cols() == pose_t::static_size + 1);
322 
323  // load into the path:
324  const size_t N = M.rows();
325  pose_t p;
326  for (size_t i = 0; i < N; i++)
327  {
328  for (unsigned int k = 0; k < pose_t::static_size; k++)
329  p[k] = M(i, k + 1);
330  insert(mrpt::system::time_tToTimestamp(M(i, 0)), p);
331  }
332  return true;
333  MRPT_END
334 }
335 
336 template <int DIM>
338  point_t& Min, point_t& Max) const
339 {
340  MRPT_START
341  ASSERT_(!m_path.empty());
342 
343  for (unsigned int k = 0; k < point_t::static_size; k++)
344  {
345  Min[k] = std::numeric_limits<double>::max();
346  Max[k] = -std::numeric_limits<double>::max();
347  }
348 
349  for (auto p = m_path.begin(); p != m_path.end(); ++p)
350  {
351  for (unsigned int k = 0; k < point_t::static_size; k++)
352  {
353  mrpt::keep_min(Min[k], p->second[k]);
354  mrpt::keep_max(Max[k], p->second[k]);
355  }
356  }
357  MRPT_END
358 }
359 
360 template <int DIM>
363 {
364  m_method = method;
365 }
366 
367 template <int DIM>
369 {
370  return m_method;
371 }
372 
373 template <int DIM>
375  unsigned int component, unsigned int samples)
376 {
377  if (m_path.empty()) return;
378 
379  TPath aux;
380 
381  int ant, post;
382  size_t nitems = size();
383 
384  post = (samples % 2) ? (unsigned int)(samples / 2) : samples / 2;
385  ant = (unsigned int)(samples / 2);
386 
387  int k = 0;
388  iterator it1, it2, it3;
389 
390  // int asamples;
391  for (it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k)
392  {
393  it2 = m_path.begin();
394  if (k - ant > 0) advance(it2, k - ant);
395 
396  if (k + post < (int)nitems)
397  {
398  it3 = m_path.begin();
399  advance(it3, k + post + 1);
400  }
401  else
402  {
403  it3 = m_path.end();
404  }
405 
406  unsigned int nsamples = distance(it2, it3);
407  CPose3DPDFParticles particles(nsamples);
408  for (unsigned int i = 0; it2 != it3; ++it2, ++i)
409  {
410  particles.m_particles[i].log_w = 0;
411  particles.m_particles[i].d = it1->second;
412  switch (component)
413  {
414  case 0:
415  particles.m_particles[i].d.x = it2->second[0];
416  break;
417  case 1:
418  particles.m_particles[i].d.y = it2->second[1];
419  break;
420  case 2:
421  particles.m_particles[i].d.z = it2->second[2];
422  break;
423  case 3:
424  particles.m_particles[i].d.yaw = it2->second[3];
425  break;
426  case 4:
427  particles.m_particles[i].d.pitch = it2->second[4];
428  break;
429  case 5:
430  particles.m_particles[i].d.roll = it2->second[5];
431  break;
432  } // end switch
433  } // end for it2
434 
435  mrpt::poses::CPose3D auxPose;
436  particles.getMean(auxPose);
437  aux[it1->first] = pose_t(auxPose.asTPose());
438  } // end for it1
439  m_path = aux;
440 }
441 } // namespace mrpt::poses
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:765
Clock that is compatible with MRPT TTimeStamp representation.
Definition: Clock.h:19
std::chrono::duration< rep, period > duration
Definition: Clock.h:25
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble t
Definition: glext.h:3695
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
Base class for SE(2)/SE(3) interpolators.
std::chrono::time_point< Clock > time_point
Definition: Clock.h:26
typename Lie::Euclidean< DIM >::light_type point_t
TPoint2D or TPoint3D.
CParticleList m_particles
The array of particles.
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
T interpolate(const T &x, const VECTOR &ys, const T &x0, const T &x1)
Interpolate a data sequence "ys" ranging from "x0" to "x1" (equally spaced), to obtain the approximat...
Definition: interp_fit.hpp:17
STL namespace.
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
GLdouble s
Definition: glext.h:3682
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
GLsizei samples
Definition: glext.h:8217
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
typename Lie::SE< DIM >::type cpose_t
CPose2D or CPose3D.
double timestampTotime_t(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.h:105
std::map< mrpt::Clock::time_point, pose_t > TPath
GLsizei const GLchar ** string
Definition: glext.h:4116
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
#define MRPT_END
Definition: exceptions.h:245
iterator find(const mrpt::Clock::time_point &t)
GLsizeiptr size
Definition: glext.h:3934
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
mrpt::system::TTimeStamp time_tToTimestamp(const double t)
Transform from standard "time_t" (actually a double number, it can contain fractions of seconds) to T...
Definition: datetime.h:91
GLfloat GLfloat p
Definition: glext.h:6398
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:182
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
GLenum filter
Definition: glext.h:5134
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1889
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
CONTAINER::Scalar norm(const CONTAINER &v)
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019