Go to the documentation of this file.
36 std::vector<mrpt::poses::CPose3D>
p;
48 pose.setFromValues(1, 1, 0, 0, 1, 0);
52 pose.setFromValues(2, 2.5, 1, 0, 1.3, 0);
56 pose.setFromValues(3, 1.7, 2, 0, 1.57, 0);
60 for (i = 0, ots = iniTs; ots <= iniTs + 3e7; ots += 1e6, i++)
65 f,
"%d %f %f %f %f %f %f\n", i, outPose.x(), outPose.y(),
66 outPose.z(), outPose.yaw(), outPose.pitch(), outPose.roll());
96 for (
t =
x[1];
t <=
x[2];
t += 0.01)
101 ys_interp.push_back(
w);
106 cout <<
"Done" << endl;
108 #if MRPT_HAS_WXWIDGETS
110 figure.plot(
x,
y,
"r.4",
"true points");
111 figure.plot(ts, ys_interp,
"b",
"interp");
137 bool valid =
interp.queryVector(xs, ys);
140 #if MRPT_HAS_WXWIDGETS
142 figure.plot(data_x, data_y,
"r.6",
"true points");
143 figure.plot(xs, ys,
"b",
"interp");
152 int main(
int argc,
char** argv)
162 catch (std::exception& e)
164 std::cout <<
"MRPT exception caught: " << e.what() << std::endl;
169 printf(
"Another exception!!");
A (persistent) sequence of (x,y) coordinates, allowing queries of intermediate points through spline ...
int void fclose(FILE *f)
An OS-independent version of fclose.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
NUMTYPE spline(const NUMTYPE t, const VECTORLIKE &x, const VECTORLIKE &y, bool wrap2pi=false)
Interpolates the value of a function in a point "t" given 4 SORTED points where "t" is between the tw...
void drawGaussian1DVector(VEC &v, const double mean=0, const double std=1)
Fills the given vector with independent, 1D-normally distributed samples.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
GLubyte GLubyte GLubyte GLubyte w
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.
#define ASSERT_(f)
Defines an assertion mechanism.
void TestSplineInterpolation()
This class stores a time-stamped trajectory in SE(3) (CPose3D poses).
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1,...
void insert(mrpt::system::TTimeStamp t, const pose_t &p)
Inserts a new pose in the sequence.
void linspace(T first, T last, size_t count, VECTOR &out_vector)
Generates an equidistant sequence of numbers given the first one, the last one and the desired number...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Classes for creating GUI windows for 2D and 3D visualization.
Create a GUI window and display plots with MATLAB-like interfaces and commands.
void TestSplineInterpolationVector()
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void setMaxTimeInterpolation(double time)
Set value of the maximum time to consider interpolation.
This base provides a set of functions for maths stuff.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
A namespace of pseudo-random numbers generators of diferent distributions.
void TestCPose3DInterpolation()
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |