Go to the documentation of this file.
29 : maxTimeInterpolation(std::chrono::seconds(-1))
44 m_path[t] = p.asTPose();
60 bool& out_valid_interp)
const
72 bool& out_valid_interp)
const
75 for (
size_t k = 0; k < pose_t::static_size; k++)
80 p1.second = p2.second = p3.second = p4.second = out_interp;
85 bool interp_method_requires_4pts;
91 interp_method_requires_4pts =
false;
94 interp_method_requires_4pts =
true;
99 auto it_ge1 = m_path.lower_bound(t);
102 if (it_ge1 != m_path.end() && it_ge1->first == t)
104 out_interp = it_ge1->second;
105 out_valid_interp =
true;
110 if (it_ge1 == m_path.end() || it_ge1 == m_path.begin())
112 out_valid_interp =
false;
117 auto it_ge2 = it_ge1;
119 if (it_ge2 == m_path.end())
121 if (interp_method_requires_4pts)
123 out_valid_interp =
false;
134 if (it_ge1 == m_path.begin())
136 if (interp_method_requires_4pts)
138 out_valid_interp =
false;
150 ? (p2.first - p1.first)
154 ? (p4.first - p3.first)
157 if (maxTimeInterpolation.count() > 0 &&
158 (dt12 > maxTimeInterpolation || dt23 > maxTimeInterpolation ||
159 dt34 > maxTimeInterpolation))
161 out_valid_interp =
false;
173 impl_interpolation(p1, p2, p3, p4, m_method, t, out_interp);
175 out_valid_interp =
true;
185 bool ret = getPreviousPoseWithMinDistance(t,
distance, p);
194 if (m_path.size() == 0 ||
distance <= 0)
return false;
199 auto it = m_path.find(t);
200 if (it != m_path.end() && it != m_path.begin())
210 }
while (d <
distance && it != m_path.begin());
214 out_pose = it->second;
226 maxTimeInterpolation = time;
232 return maxTimeInterpolation;
242 if (!f.is_open())
return false;
244 for (
auto i = m_path.begin(); i != m_path.end(); ++i)
247 const auto& p = i->second;
250 for (
unsigned int k = 0; k < p.size(); k++)
252 str += std::string(
"\n");
273 if (!f.is_open())
return false;
274 if (m_path.empty())
return true;
286 if (!valid)
continue;
289 for (
unsigned int k = 0; k < p.size(); k++)
291 str += std::string(
"\n");
314 catch (std::exception&)
320 if (M.
rows() == 0)
return false;
324 const size_t N = M.
rows();
326 for (
size_t i = 0; i < N; i++)
328 for (
unsigned int k = 0; k < pose_t::static_size; k++)
343 for (
unsigned int k = 0; k < point_t::static_size; k++)
345 Min[k] = std::numeric_limits<double>::max();
346 Max[k] = -std::numeric_limits<double>::max();
349 for (
auto p = m_path.begin(); p != m_path.end(); ++p)
351 for (
unsigned int k = 0; k < point_t::static_size; k++)
375 unsigned int component,
unsigned int samples)
377 if (m_path.empty())
return;
382 size_t nitems =
size();
384 post = (samples % 2) ? (
unsigned int)(samples / 2) : samples / 2;
385 ant = (
unsigned int)(samples / 2);
391 for (it1 = m_path.begin(); it1 != m_path.end(); ++it1, ++k)
393 it2 = m_path.begin();
394 if (k - ant > 0) advance(it2, k - ant);
396 if (k + post < (
int)nitems)
398 it3 = m_path.begin();
399 advance(it3, k + post + 1);
406 unsigned int nsamples =
distance(it2, it3);
408 for (
unsigned int i = 0; it2 != it3; ++it2, ++i)
typename Lie::Euclidean< DIM >::light_type point_t
TPoint2D or TPoint3D.
bool loadFromTextFile(const std::string &s)
Loads from a text file, in the format described by saveToTextFile.
void clear()
Clear the contents of this container.
std::chrono::duration< rep, period > duration
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...
typename Lie::SE< DIM >::light_type pose_t
TPose2D or TPose3D.
void loadFromTextFile(std::istream &f)
Loads a vector/matrix from a text file, compatible with MATLAB text format.
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...
typename Lie::SE< DIM >::type cpose_t
CPose2D or CPose3D.
bool getPreviousPoseWithMinDistance(const mrpt::Clock::time_point &t, double distance, pose_t &out_pose)
Get the previous CPose3D in the map with a minimum defined distance.
CPoseInterpolatorBase()
Default ctor: empty sequence of poses.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
typename TPath::iterator iterator
pose_t & interpolate(const mrpt::Clock::time_point &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.
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void setInterpolationMethod(TInterpolatorMethod method)
Change the method used to interpolate the robot path.
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...
void insert(const mrpt::Clock::time_point &t, const pose_t &p)
Inserts a new pose in the sequence.
CONTAINER::Scalar norm(const CONTAINER &v)
TInterpolatorMethod
Type to select the interpolation method in CPoseInterpolatorBase derived classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::math::TPose3D asTPose() const
void clear()
Clears the current sequence of poses.
Clock that is compatible with MRPT TTimeStamp representation.
size_t size(const MATRIXLIKE &m, const int dim)
TInterpolatorMethod getInterpolationMethod() const
Returns the currently set interpolation method.
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.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
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.
std::chrono::time_point< Clock > time_point
CParticleList m_particles
The array of particles.
void getBoundingBox(point_t &minCorner, point_t &maxCorner) const
Computes the bounding box in all Euclidean coordinates of the whole path.
double timestampTotime_t(const mrpt::system::TTimeStamp t) noexcept
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
void filter(unsigned int component, unsigned int samples)
Filters by averaging one of the components of the pose data within the interpolator.
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF),...
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
size_type cols() const
Number of columns in the matrix.
mrpt::Clock::duration getMaxTimeInterpolation()
Set value of the maximum time to consider interpolation.
bool saveInterpolatedToTextFile(const std::string &s, const mrpt::Clock::duration &period) const
Saves the points in the interpolator to a text file, with the same format that saveToTextFile,...
std::map< mrpt::Clock::time_point, pose_t > TPath
size_type rows() const
Number of rows in the matrix.
std::pair< mrpt::Clock::time_point, pose_t > TTimePosePair
std::string std::string format(std::string_view fmt, ARGS &&... args)
void setMaxTimeInterpolation(const mrpt::Clock::duration &time)
Set value of the maximum time to consider interpolation.
Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020 | |