26 maxTimeInterpolation(
std::chrono::seconds(-1)),
40 m_path[
t] =
p.asTPose();
68 p1.second = p2.second = p3.second = p4.second = out_interp;
72 bool interp_method_requires_4pts;
78 interp_method_requires_4pts =
false;
81 interp_method_requires_4pts =
true;
90 if( it_ge1 != m_path.end() && it_ge1->first ==
t )
92 out_interp = it_ge1->second;
93 out_valid_interp =
true;
98 if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
100 out_valid_interp =
false;
106 if(it_ge2 == m_path.end() )
108 if (interp_method_requires_4pts) {
109 out_valid_interp =
false;
119 if( it_ge1 == m_path.begin() )
121 if (interp_method_requires_4pts) {
122 out_valid_interp =
false;
135 if( maxTimeInterpolation.count() > 0 &&
136 (dt12 > maxTimeInterpolation ||
137 dt23 > maxTimeInterpolation ||
138 dt34 > maxTimeInterpolation ))
140 out_valid_interp =
false;
152 impl_interpolation(p1,p2,p3,p4, m_method,
t,out_interp);
154 out_valid_interp =
true;
163 bool ret = getPreviousPoseWithMinDistance(
t,
distance,
p);
171 if( m_path.size() == 0 ||
distance <=0 )
178 if( it != m_path.end() && it != m_path.begin() )
188 }
while( d <
distance && it != m_path.begin() );
192 out_pose = it->second;
203 maxTimeInterpolation = time;
209 return maxTimeInterpolation;
219 if (!f.is_open())
return false;
224 const auto &
p = i->second;
227 for (
unsigned int k=0;k<
p.size();k++)
249 if (!f.is_open())
return false;
250 if (m_path.empty())
return true;
262 if (!valid)
continue;
265 for (
unsigned int k=0;k<
p.size();k++)
288 M.loadFromTextFile(
s);
290 catch(std::exception &)
296 if (M.rows()==0)
return false;
300 const size_t N = M.cols();
302 for (
size_t i=0;i<N;i++) {
319 Min[k] = std::numeric_limits<double>::max();
320 Max[k] =-std::numeric_limits<double>::max();
354 size_t nitems =
size();
357 ant = (
unsigned int)(
samples/2);
363 for( it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k )
365 it2 = m_path.begin();
367 advance( it2, k-ant );
369 if( k+post < (
int)nitems )
371 it3 = m_path.begin();
372 advance( it3, k+post+1 );
379 unsigned int nsamples =
distance(it2,it3);
381 for(
unsigned int i = 0; it2 != it3; ++it2, ++i )
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;
394 case 5: particles.
m_particles[i].d.roll= it2->second[5];
break;
mrpt::math::TPose3D asTPose() const
Clock that is compatible with MRPT TTimeStamp representation.
std::chrono::duration< rep, period > duration
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Base class for SE(2)/SE(3) interpolators.
std::chrono::time_point< Clock > time_point
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...
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...
typename mrpt::poses::SE_traits< DIM >::lightweight_pose_t pose_t
TPose2D or TPose3D.
#define ASSERT_(f)
Defines an assertion mechanism.
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...
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
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.
typename TPath::iterator iterator
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).
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.
typename TPath::const_iterator const_iterator
iterator find(const mrpt::Clock::time_point &t)
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...
void clear()
Clear the contents of this container.
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes. ...
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
CONTAINER::Scalar norm(const CONTAINER &v)