Main MRPT website > C++ reference for MRPT 1.5.7
CPoseInterpolatorBase.hpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
11 
14 #include <mrpt/math/slerp.h>
15 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/math/interp_fit.hpp>
17 #include <mrpt/math/CMatrixD.h>
19 
20 namespace mrpt {
21 namespace poses {
22 
23 template <int DIM>
25 {
26  maxTimeInterpolation = -1.0;
27 }
28 
29 template <int DIM>
31 {
32  m_path.clear();
33 }
34 
35 template <int DIM>
37 {
38  m_path[t] = pose_t(p);
39 }
40 template <int DIM>
42 {
43  m_path[t] = p;
44 }
45 
46 /*---------------------------------------------------------------
47  interpolate
48  ---------------------------------------------------------------*/
49 template <int DIM>
51 {
52  pose_t p;
53  this->interpolate(t, p, out_valid_interp);
54  out_interp = cpose_t(p);
55  return out_interp;
56 }
57 
58 template <int DIM>
60 {
61  // Default value in case of invalid interp
62  for (size_t k=0;k<pose_t::static_size;k++) {
63  out_interp[k]=0;
64  }
65  TTimePosePair p1, p2, p3, p4;
66  p1.second = p2.second = p3.second = p4.second = out_interp;
67 
68  // Invalid?
69  if (t==INVALID_TIMESTAMP)
70  {
71  out_valid_interp = false;
72  return out_interp;
73  }
74 
75  // We'll look for 4 consecutive time points.
76  // Check if the selected method needs all 4 points or just the central 2 of them:
77  bool interp_method_requires_4pts;
78  switch (m_method)
79  {
80  case imLinear2Neig:
81  case imSplineSlerp:
82  case imLinearSlerp:
83  interp_method_requires_4pts = false;
84  break;
85  default:
86  interp_method_requires_4pts = true;
87  break;
88  };
89 
90 
91  // Out of range?
92  const_iterator it_ge1 = m_path.lower_bound( t );
93 
94  // Exact match?
95  if( it_ge1 != m_path.end() && it_ge1->first == t )
96  {
97  out_interp = it_ge1->second;
98  out_valid_interp = true;
99  return out_interp;
100  }
101 
102  // Are we in the beginning or the end of the path?
103  if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
104  {
105  out_valid_interp = false;
106  return out_interp;
107  } // end
108 
109  p3 = *it_ge1; // Third pair
110  const_iterator it_ge2 = it_ge1; ++it_ge2;
111  if(it_ge2 == m_path.end() )
112  {
113  if (interp_method_requires_4pts) {
114  out_valid_interp = false;
115  return out_interp;
116  }
117  }
118  else {
119  p4 = *(it_ge2); // Fourth pair
120  }
121 
122  p2 = *(--it_ge1); // Second pair
123 
124  if( it_ge1 == m_path.begin() )
125  {
126  if (interp_method_requires_4pts) {
127  out_valid_interp = false;
128  return out_interp;
129  }
130  }
131  else {
132  p1 = *(--it_ge1); // First pair
133  }
134 
135  // Test if the difference between the desired timestamp and the next timestamp is lower than a certain (configurable) value
136  const double dt12 = interp_method_requires_4pts ? (p2.first - p1.first) / 1e7 : .0;
137  const double dt23 = (p3.first - p2.first) / 1e7;
138  const double dt34 = interp_method_requires_4pts ? (p4.first - p3.first) / 1e7 : .0;
139 
140  if( maxTimeInterpolation > 0 &&
141  (dt12 > maxTimeInterpolation ||
142  dt23 > maxTimeInterpolation ||
143  dt34 > maxTimeInterpolation ))
144  {
145  out_valid_interp = false;
146  return out_interp;
147  }
148 
149  // Do interpolation:
150  // ------------------------------------------
151  // First Previous point: p1
152  // Second Previous point: p2
153  // First Next point: p3
154  // Second Next point: p4
155  // Time where to interpolate: t
156  double td = mrpt::system::timestampTotime_t(t);
157 
159  ts[0] = mrpt::system::timestampTotime_t(p1.first);
160  ts[1] = mrpt::system::timestampTotime_t(p2.first);
161  ts[2] = mrpt::system::timestampTotime_t(p3.first);
162  ts[3] = mrpt::system::timestampTotime_t(p4.first);
163 
164  impl_interpolation(ts,p1,p2,p3,p4, m_method,td,out_interp);
165 
166  out_valid_interp = true;
167  return out_interp;
168 
169 } // end interpolate
170 
171 template <int DIM>
173 {
174  pose_t p;
175  bool ret = getPreviousPoseWithMinDistance(t, distance, p);
176  out_pose = cpose_t(p);
177  return ret;
178 }
179 
180 template <int DIM>
182 {
183  if( m_path.size() == 0 || distance <=0 )
184  return false;
185 
186  pose_t myPose;
187 
188  // Search for the desired timestamp
189  iterator it = m_path.find(t);
190  if( it != m_path.end() && it != m_path.begin() )
191  myPose = it->second;
192  else
193  return false;
194 
195  double d = 0.0;
196  do
197  {
198  --it;
199  d = (point_t(myPose) - point_t(it->second)).norm();
200  } while( d < distance && it != m_path.begin() );
201 
202  if( d >= distance )
203  {
204  out_pose = it->second;
205  return true;
206  }
207  else
208  return false;
209 } // end getPreviousPose
210 
211 template <int DIM>
213 {
214  ASSERT_( time > 0 );
215  maxTimeInterpolation = time;
216 }
217 
218 template <int DIM>
220 {
221  return maxTimeInterpolation;
222 }
223 
224 template <int DIM>
226 {
227  try
228  {
230  std::string str;
231  for (const_iterator i=m_path.begin();i!=m_path.end();++i)
232  {
233  const double t = mrpt::system::timestampTotime_t(i->first);
234  const auto &p = i->second;
235 
236  str = mrpt::format("%.06f ",t);
237  for (unsigned int k=0;k<p.size();k++)
238  str+= mrpt::format("%.06f ",p[k]);
239  str+= std::string("\n");
240 
241  f.printf("%s",str.c_str());
242  }
243  return true;
244  }
245  catch(...)
246  {
247  return false;
248  }
249 }
250 
251 template <int DIM>
253 {
255  try
256  {
258  if (m_path.empty()) return true;
259 
260  std::string str;
261 
262  const TTimeStamp t_ini = m_path.begin()->first;
263  const TTimeStamp t_end = m_path.rbegin()->first;
264 
266 
267  pose_t p;
268  bool valid;
269  for (TTimeStamp t=t_ini;t<=t_end;t+=At)
270  {
271  this->interpolate( t, p, valid );
272  if (!valid) continue;
273 
275  for (unsigned int k=0;k<p.size();k++)
276  str+= mrpt::format("%.06f ",p[k]);
277  str+= std::string("\n");
278  f.printf("%s",str.c_str());
279  }
280  return true;
281  }
282  catch(...)
283  {
284  return false;
285  }
286 }
287 
288 template <int DIM>
290 {
291  MRPT_START
292 
293  clear();
295 
296  try
297  {
298  M.loadFromTextFile(s);
299  }
300  catch(std::exception &)
301  {
302  return false; // error loading file
303  }
304 
305  // Check valid format:
306  if (M.getRowCount()==0) return false;
307  ASSERT_(M.getColCount()== pose_t::static_size+1 );
308 
309  // load into the path:
310  const size_t N = M.getColCount();
311  pose_t p;
312  for (size_t i=0;i<N;i++) {
313  for (unsigned int k=0;k<pose_t::static_size;k++)
314  p[k] = M(i,k+1);
315  insert(mrpt::system::time_tToTimestamp( M(i,0) ), p );
316  }
317  return true;
318  MRPT_END
319 }
320 
321 
322 template <int DIM>
324 {
325  MRPT_START
326  ASSERT_( !m_path.empty() );
327 
328  for (unsigned int k=0;k<point_t::static_size;k++) {
329  Min[k] = std::numeric_limits<double>::max();
330  Max[k] =-std::numeric_limits<double>::max();
331  }
332 
333  for (const_iterator p=m_path.begin();p!=m_path.end();++p)
334  {
335  for (unsigned int k=0;k<point_t::static_size;k++) {
336  mrpt::utils::keep_min( Min[k], p->second[k]);
337  mrpt::utils::keep_max( Max[k], p->second[k]);
338  }
339  }
340  MRPT_END
341 }
342 
343 template <int DIM>
345 {
346  m_method = method;
347 }
348 
349 template <int DIM>
351 {
352  return m_method;
353 }
354 
355 template <int DIM>
356 void CPoseInterpolatorBase<DIM>::filter( unsigned int component, unsigned int samples )
357 {
358  if (m_path.empty())
359  return;
360 
361  TPath aux;
362 
363  int ant, post;
364  size_t nitems = size();
365 
366  post = (samples%2) ? (unsigned int)(samples/2) : samples/2;
367  ant = (unsigned int)(samples/2);
368 
369  int k = 0;
370  iterator it1, it2, it3;
371 
372  //int asamples;
373  for( it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k )
374  {
375  it2 = m_path.begin();
376  if( k-ant > 0 )
377  advance( it2, k-ant );
378 
379  if( k+post < (int)nitems )
380  {
381  it3 = m_path.begin();
382  advance( it3, k+post+1 );
383  }
384  else
385  {
386  it3 = m_path.end();
387  }
388 
389  unsigned int nsamples = distance(it2,it3);
390  CPose3DPDFParticles particles(nsamples);
391  for( unsigned int i = 0; it2 != it3; ++it2, ++i )
392  {
393  particles.m_particles[i].log_w = 0;
394  *particles.m_particles[i].d = mrpt::poses::CPose3D(it1->second);
395  switch( component )
396  {
397  case 0: particles.m_particles[i].d->x(it2->second[0]); break;
398  case 1: particles.m_particles[i].d->y(it2->second[1]); break;
399  case 2: particles.m_particles[i].d->z(it2->second[2]); break;
400  case 3: particles.m_particles[i].d->setYawPitchRoll(it2->second[3],it1->second[4],it1->second[5]); break;
401  case 4: particles.m_particles[i].d->setYawPitchRoll(it1->second[3],it2->second[4],it1->second[5]); break;
402  case 5: particles.m_particles[i].d->setYawPitchRoll(it1->second[3],it1->second[4],it2->second[5]); break;
403  } // end switch
404  } // end for it2
405 
406  mrpt::poses::CPose3D auxPose;
407  particles.getMean( auxPose );
408  aux[it1->first] = pose_t(auxPose);
409  } // end for it1
410  m_path = aux;
411 } // end filter
412 
413 } // end ns
414 } // end ns
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
GLdouble GLdouble t
Definition: glext.h:3610
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
Base class for SE(2)/SE(3) interpolators.
std::map< mrpt::system::TTimeStamp, pose_t > TPath
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
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
GLdouble s
Definition: glext.h:3602
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
CParticleList m_particles
The array of particles.
GLsizei samples
Definition: glext.h:6928
#define MRPT_END
This CStream derived class allow using a file as a write-only, binary stream.
iterator find(const mrpt::system::TTimeStamp &t)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLsizei const GLchar ** string
Definition: glext.h:3919
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...
Definition: bits.h:176
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
#define MRPT_START
void loadFromTextFile(const std::string &file)
Load matrix from a text file, compatible with MATLAB text format.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
Definition: bits.h:171
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::poses::SE_traits< DIM >::point_t point_t
TPoint2D or TPoint3D.
#define ASSERT_(f)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
GLsizeiptr size
Definition: glext.h:3779
mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
mrpt::system::TTimeStamp BASE_IMPEXP time_tToTimestamp(const double t)
Transform from standard "time_t" (actually a double number, it can contain fractions of seconds) to T...
Definition: datetime.cpp:48
GLfloat GLfloat p
Definition: glext.h:5587
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
GLenum filter
Definition: glext.h:4617
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
double BASE_IMPEXP timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.cpp:53
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019