Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
13 #include <mrpt/math/slerp.h>
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/math/interp_fit.hpp>
16 #include <mrpt/math/CMatrixD.h>
18 #include <fstream>
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] = p.asTPose();
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  {
229  std::ofstream f;
230  f.open(s);
231  if (!f.is_open()) return false;
232  std::string str;
233  for (const_iterator i=m_path.begin();i!=m_path.end();++i)
234  {
235  const double t = mrpt::system::timestampTotime_t(i->first);
236  const auto &p = i->second;
237 
238  str = mrpt::format("%.06f ",t);
239  for (unsigned int k=0;k<p.size();k++)
240  str+= mrpt::format("%.06f ",p[k]);
241  str+= std::string("\n");
242 
243  f << str;
244  }
245  return true;
246  }
247  catch(...)
248  {
249  return false;
250  }
251 }
252 
253 template <int DIM>
255 {
257  try
258  {
259  std::ofstream f;
260  f.open(s);
261  if (!f.is_open()) return false;
262  if (m_path.empty()) return true;
263 
264  std::string str;
265 
266  const TTimeStamp t_ini = m_path.begin()->first;
267  const TTimeStamp t_end = m_path.rbegin()->first;
268 
270 
271  pose_t p;
272  bool valid;
273  for (TTimeStamp t=t_ini;t<=t_end;t+=At)
274  {
275  this->interpolate( t, p, valid );
276  if (!valid) continue;
277 
279  for (unsigned int k=0;k<p.size();k++)
280  str+= mrpt::format("%.06f ",p[k]);
281  str+= std::string("\n");
282  f << str;
283  }
284  return true;
285  }
286  catch(...)
287  {
288  return false;
289  }
290 }
291 
292 template <int DIM>
294 {
295  MRPT_START
296 
297  clear();
299 
300  try
301  {
302  M.loadFromTextFile(s);
303  }
304  catch(std::exception &)
305  {
306  return false; // error loading file
307  }
308 
309  // Check valid format:
310  if (M.rows()==0) return false;
311  ASSERT_(M.cols()== pose_t::static_size+1 );
312 
313  // load into the path:
314  const size_t N = M.cols();
315  pose_t p;
316  for (size_t i=0;i<N;i++) {
317  for (unsigned int k=0;k<pose_t::static_size;k++)
318  p[k] = M(i,k+1);
319  insert(mrpt::system::time_tToTimestamp( M(i,0) ), p );
320  }
321  return true;
322  MRPT_END
323 }
324 
325 
326 template <int DIM>
328 {
329  MRPT_START
330  ASSERT_( !m_path.empty() );
331 
332  for (unsigned int k=0;k<point_t::static_size;k++) {
333  Min[k] = std::numeric_limits<double>::max();
334  Max[k] =-std::numeric_limits<double>::max();
335  }
336 
337  for (const_iterator p=m_path.begin();p!=m_path.end();++p)
338  {
339  for (unsigned int k=0;k<point_t::static_size;k++) {
340  mrpt::keep_min( Min[k], p->second[k]);
341  mrpt::keep_max( Max[k], p->second[k]);
342  }
343  }
344  MRPT_END
345 }
346 
347 template <int DIM>
349 {
350  m_method = method;
351 }
352 
353 template <int DIM>
355 {
356  return m_method;
357 }
358 
359 template <int DIM>
360 void CPoseInterpolatorBase<DIM>::filter( unsigned int component, unsigned int samples )
361 {
362  if (m_path.empty())
363  return;
364 
365  TPath aux;
366 
367  int ant, post;
368  size_t nitems = size();
369 
370  post = (samples%2) ? (unsigned int)(samples/2) : samples/2;
371  ant = (unsigned int)(samples/2);
372 
373  int k = 0;
374  iterator it1, it2, it3;
375 
376  //int asamples;
377  for( it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k )
378  {
379  it2 = m_path.begin();
380  if( k-ant > 0 )
381  advance( it2, k-ant );
382 
383  if( k+post < (int)nitems )
384  {
385  it3 = m_path.begin();
386  advance( it3, k+post+1 );
387  }
388  else
389  {
390  it3 = m_path.end();
391  }
392 
393  unsigned int nsamples = distance(it2,it3);
394  CPose3DPDFParticles particles(nsamples);
395  for( unsigned int i = 0; it2 != it3; ++it2, ++i )
396  {
397  particles.m_particles[i].log_w = 0;
398  particles.m_particles[i].d = it1->second;
399  switch( component )
400  {
401  case 0: particles.m_particles[i].d.x= it2->second[0]; break;
402  case 1: particles.m_particles[i].d.y=it2->second[1]; break;
403  case 2: particles.m_particles[i].d.z=it2->second[2]; break;
404  case 3: particles.m_particles[i].d.yaw = it2->second[3]; break;
405  case 4:
406  particles.m_particles[i].d.pitch = it2->second[4];
407  break;
408  case 5: particles.m_particles[i].d.roll= it2->second[5]; break;
409  } // end switch
410  } // end for it2
411 
412  mrpt::poses::CPose3D auxPose;
413  particles.getMean( auxPose );
414  aux[it1->first] = pose_t(auxPose.asTPose());
415  } // end for it1
416  m_path = aux;
417 } // end filter
418 
419 } // end ns
420 } // end ns
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
#define MRPT_START
Definition: exceptions.h:262
GLdouble GLdouble t
Definition: glext.h:3689
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:24
Base class for SE(2)/SE(3) interpolators.
CParticleList m_particles
The array of particles.
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:19
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
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...
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:3676
typename mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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:8068
std::pair< mrpt::system::TTimeStamp, pose_t > TTimePosePair
iterator find(const mrpt::system::TTimeStamp &t)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
typename mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
GLsizei const GLchar ** string
Definition: glext.h:4101
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
typename mrpt::poses::SE_traits< DIM >::point_t point_t
TPoint2D or TPoint3D.
std::map< mrpt::system::TTimeStamp, pose_t > TPath
typename TPath::const_iterator const_iterator
#define MRPT_END
Definition: exceptions.h:266
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
mrpt::system::TTimeStamp secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:224
GLsizeiptr size
Definition: glext.h:3923
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.cpp:50
GLfloat GLfloat p
Definition: glext.h:6305
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
GLenum filter
Definition: glext.h:5072
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
double 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:56
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.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019