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);
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
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.
void getBoundingBox(point_t &minCorner, point_t &maxCorner) const
Computes the bounding box in all Euclidean coordinates of the whole path.
std::map< mrpt::system::TTimeStamp, pose_t > TPath
GLdouble GLdouble t
Definition: glew.h:1303
TPath m_path
The sequence of poses.
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path. The default method at construction is "imSpline...
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...
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the pose data within the interpolator.
GLdouble s
Definition: glew.h:1295
CParticleList m_particles
The array of particles.
pose_t & interpolate(mrpt::system::TTimeStamp t, pose_t &out_interp, bool &out_valid_interp) const
Returns the pose at a given time, or interpolates using splines if there is not an exact match...
void insert(mrpt::system::TTimeStamp t, const pose_t &p)
Inserts a new pose in the sequence.
#define MRPT_END
This CStream derived class allow using a file as a write-only, binary stream.
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by saveToTextFile.
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
void setMaxTimeInterpolation(double time)
Set value of the maximum time to consider interpolation.
bool saveToTextFile(const std::string &s) const
Saves the points in the interpolator to a text file, with this format: Each row contains these elemen...
#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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void clear()
Clears the current sequence of poses.
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.
double getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
#define ASSERT_(f)
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
bool getPreviousPoseWithMinDistance(const mrpt::system::TTimeStamp &t, double distance, pose_t &out_pose)
Get the previous CPose3D in the map with a minimum defined distance.
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
double maxTimeInterpolation
Maximum time considered to interpolate. If the difference between the desired timestamp where to inte...
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
GLsizei samples
Definition: glew.h:2695
bool saveInterpolatedToTextFile(const std::string &s, double period) const
Saves the points in the interpolator to a text file, with the same format that saveToTextFile, but interpolating the path with the given period in seconds.
void impl_interpolation(const mrpt::math::CArrayDouble< 4 > &ts, const TTimePosePair p1, const TTimePosePair p2, const TTimePosePair p3, const TTimePosePair p4, const TInterpolatorMethod method, double td, pose_t &out_interp) const
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...
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1511
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)
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: cdb1297 Tue Jun 12 13:44:11 2018 +0200 at mar jun 12 15:30:13 CEST 2018