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 <mrpt/system/datetime.h>
19 #include <fstream>
20 
21 namespace mrpt::poses
22 {
23 
24 template <int DIM>
26  maxTimeInterpolation(std::chrono::seconds(-1)),
27  m_method( mrpt::poses::imLinearSlerp )
28 {
29 }
30 
31 template <int DIM>
33 {
34  m_path.clear();
35 }
36 
37 template <int DIM>
39 {
40  m_path[t] = p.asTPose();
41 }
42 template <int DIM>
44 {
45  m_path[t] = p;
46 }
47 
48 /*---------------------------------------------------------------
49  interpolate
50  ---------------------------------------------------------------*/
51 template <int DIM>
53 {
54  pose_t p;
55  this->interpolate(t, p, out_valid_interp);
56  out_interp = cpose_t(p);
57  return out_interp;
58 }
59 
60 template <int DIM>
62 {
63  // Default value in case of invalid interp
64  for (size_t k=0;k<pose_t::static_size;k++) {
65  out_interp[k]=0;
66  }
67  TTimePosePair p1, p2, p3, p4;
68  p1.second = p2.second = p3.second = p4.second = out_interp;
69 
70  // We'll look for 4 consecutive time points.
71  // Check if the selected method needs all 4 points or just the central 2 of them:
72  bool interp_method_requires_4pts;
73  switch (m_method)
74  {
75  case imLinear2Neig:
76  case imSplineSlerp:
77  case imLinearSlerp:
78  interp_method_requires_4pts = false;
79  break;
80  default:
81  interp_method_requires_4pts = true;
82  break;
83  };
84 
85 
86  // Out of range?
87  const_iterator it_ge1 = m_path.lower_bound( t );
88 
89  // Exact match?
90  if( it_ge1 != m_path.end() && it_ge1->first == t )
91  {
92  out_interp = it_ge1->second;
93  out_valid_interp = true;
94  return out_interp;
95  }
96 
97  // Are we in the beginning or the end of the path?
98  if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
99  {
100  out_valid_interp = false;
101  return out_interp;
102  } // end
103 
104  p3 = *it_ge1; // Third pair
105  const_iterator it_ge2 = it_ge1; ++it_ge2;
106  if(it_ge2 == m_path.end() )
107  {
108  if (interp_method_requires_4pts) {
109  out_valid_interp = false;
110  return out_interp;
111  }
112  }
113  else {
114  p4 = *(it_ge2); // Fourth pair
115  }
116 
117  p2 = *(--it_ge1); // Second pair
118 
119  if( it_ge1 == m_path.begin() )
120  {
121  if (interp_method_requires_4pts) {
122  out_valid_interp = false;
123  return out_interp;
124  }
125  }
126  else {
127  p1 = *(--it_ge1); // First pair
128  }
129 
130  // Test if the difference between the desired timestamp and the next timestamp is lower than a certain (configurable) value
131  const mrpt::Clock::duration dt12 = interp_method_requires_4pts ? (p2.first - p1.first) : mrpt::Clock::duration(0);
132  const mrpt::Clock::duration dt23 = (p3.first - p2.first);
133  const mrpt::Clock::duration dt34 = interp_method_requires_4pts ? (p4.first - p3.first) : mrpt::Clock::duration(0);
134 
135  if( maxTimeInterpolation.count() > 0 &&
136  (dt12 > maxTimeInterpolation ||
137  dt23 > maxTimeInterpolation ||
138  dt34 > maxTimeInterpolation ))
139  {
140  out_valid_interp = false;
141  return out_interp;
142  }
143 
144  // Do interpolation:
145  // ------------------------------------------
146  // First Previous point: p1
147  // Second Previous point: p2
148  // First Next point: p3
149  // Second Next point: p4
150  // Time where to interpolate: t
151 
152  impl_interpolation(p1,p2,p3,p4, m_method,t,out_interp);
153 
154  out_valid_interp = true;
155  return out_interp;
156 
157 } // end interpolate
158 
159 template <int DIM>
161 {
162  pose_t p;
163  bool ret = getPreviousPoseWithMinDistance(t, distance, p);
164  out_pose = cpose_t(p);
165  return ret;
166 }
167 
168 template <int DIM>
170 {
171  if( m_path.size() == 0 || distance <=0 )
172  return false;
173 
174  pose_t myPose;
175 
176  // Search for the desired timestamp
177  iterator it = m_path.find(t);
178  if( it != m_path.end() && it != m_path.begin() )
179  myPose = it->second;
180  else
181  return false;
182 
183  double d = 0.0;
184  do
185  {
186  --it;
187  d = (point_t(myPose) - point_t(it->second)).norm();
188  } while( d < distance && it != m_path.begin() );
189 
190  if( d >= distance )
191  {
192  out_pose = it->second;
193  return true;
194  }
195  else
196  return false;
197 } // end getPreviousPose
198 
199 template <int DIM>
201 {
202  ASSERT_( time.count() > 0 );
203  maxTimeInterpolation = time;
204 }
205 
206 template <int DIM>
208 {
209  return maxTimeInterpolation;
210 }
211 
212 template <int DIM>
214 {
215  try
216  {
217  std::ofstream f;
218  f.open(s);
219  if (!f.is_open()) return false;
220  std::string str;
221  for (const_iterator i=m_path.begin();i!=m_path.end();++i)
222  {
223  const double t = mrpt::system::timestampTotime_t(i->first);
224  const auto &p = i->second;
225 
226  str = mrpt::format("%.06f ",t);
227  for (unsigned int k=0;k<p.size();k++)
228  str+= mrpt::format("%.06f ",p[k]);
229  str+= std::string("\n");
230 
231  f << str;
232  }
233  return true;
234  }
235  catch(...)
236  {
237  return false;
238  }
239 }
240 
241 template <int DIM>
243 {
244  using mrpt::Clock;
245  try
246  {
247  std::ofstream f;
248  f.open(s);
249  if (!f.is_open()) return false;
250  if (m_path.empty()) return true;
251 
252  std::string str;
253 
254  const Clock::time_point t_ini = m_path.begin()->first;
255  const Clock::time_point t_end = m_path.rbegin()->first;
256 
257  pose_t p;
258  bool valid;
259  for (Clock::time_point t = t_ini; t <= t_end; t += period)
260  {
261  this->interpolate( t, p, valid );
262  if (!valid) continue;
263 
265  for (unsigned int k=0;k<p.size();k++)
266  str+= mrpt::format("%.06f ",p[k]);
267  str+= std::string("\n");
268  f << str;
269  }
270  return true;
271  }
272  catch(...)
273  {
274  return false;
275  }
276 }
277 
278 template <int DIM>
280 {
281  MRPT_START
282 
283  clear();
285 
286  try
287  {
288  M.loadFromTextFile(s);
289  }
290  catch(std::exception &)
291  {
292  return false; // error loading file
293  }
294 
295  // Check valid format:
296  if (M.rows()==0) return false;
297  ASSERT_(M.cols()== pose_t::static_size+1 );
298 
299  // load into the path:
300  const size_t N = M.cols();
301  pose_t p;
302  for (size_t i=0;i<N;i++) {
303  for (unsigned int k=0;k<pose_t::static_size;k++)
304  p[k] = M(i,k+1);
305  insert(mrpt::system::time_tToTimestamp(M(i,0)), p );
306  }
307  return true;
308  MRPT_END
309 }
310 
311 
312 template <int DIM>
314 {
315  MRPT_START
316  ASSERT_( !m_path.empty() );
317 
318  for (unsigned int k=0;k<point_t::static_size;k++) {
319  Min[k] = std::numeric_limits<double>::max();
320  Max[k] =-std::numeric_limits<double>::max();
321  }
322 
323  for (const_iterator p=m_path.begin();p!=m_path.end();++p)
324  {
325  for (unsigned int k=0;k<point_t::static_size;k++) {
326  mrpt::keep_min( Min[k], p->second[k]);
327  mrpt::keep_max( Max[k], p->second[k]);
328  }
329  }
330  MRPT_END
331 }
332 
333 template <int DIM>
335 {
336  m_method = method;
337 }
338 
339 template <int DIM>
341 {
342  return m_method;
343 }
344 
345 template <int DIM>
346 void CPoseInterpolatorBase<DIM>::filter( unsigned int component, unsigned int samples )
347 {
348  if (m_path.empty())
349  return;
350 
351  TPath aux;
352 
353  int ant, post;
354  size_t nitems = size();
355 
356  post = (samples%2) ? (unsigned int)(samples/2) : samples/2;
357  ant = (unsigned int)(samples/2);
358 
359  int k = 0;
360  iterator it1, it2, it3;
361 
362  //int asamples;
363  for( it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k )
364  {
365  it2 = m_path.begin();
366  if( k-ant > 0 )
367  advance( it2, k-ant );
368 
369  if( k+post < (int)nitems )
370  {
371  it3 = m_path.begin();
372  advance( it3, k+post+1 );
373  }
374  else
375  {
376  it3 = m_path.end();
377  }
378 
379  unsigned int nsamples = distance(it2,it3);
380  CPose3DPDFParticles particles(nsamples);
381  for( unsigned int i = 0; it2 != it3; ++it2, ++i )
382  {
383  particles.m_particles[i].log_w = 0;
384  particles.m_particles[i].d = it1->second;
385  switch( component )
386  {
387  case 0: particles.m_particles[i].d.x= it2->second[0]; break;
388  case 1: particles.m_particles[i].d.y=it2->second[1]; break;
389  case 2: particles.m_particles[i].d.z=it2->second[2]; break;
390  case 3: particles.m_particles[i].d.yaw = it2->second[3]; break;
391  case 4:
392  particles.m_particles[i].d.pitch = it2->second[4];
393  break;
394  case 5: particles.m_particles[i].d.roll= it2->second[5]; break;
395  } // end switch
396  } // end for it2
397 
398  mrpt::poses::CPose3D auxPose;
399  particles.getMean( auxPose );
400  aux[it1->first] = pose_t(auxPose.asTPose());
401  } // end for it1
402  m_path = aux;
403 }
404 } // end ns
405 
406 
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1043
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:262
GLdouble GLdouble t
Definition: glext.h:3689
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:22
Base class for SE(2)/SE(3) interpolators.
std::chrono::time_point< Clock > time_point
Definition: Clock.h:26
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
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...
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: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::map< mrpt::Clock::time_point, pose_t > TPath
typename mrpt::poses::SE_traits< DIM >::pose_t cpose_t
CPose2D or CPose3D.
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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:86
typename mrpt::poses::SE_traits< DIM >::point_t point_t
TPoint2D or TPoint3D.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
typename TPath::const_iterator const_iterator
#define MRPT_END
Definition: exceptions.h:266
iterator find(const mrpt::Clock::time_point &t)
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.h:93
GLfloat GLfloat p
Definition: glext.h:6305
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:186
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:50
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020