Go to the documentation of this file.
12 #include <mrpt/config.h>
21 #include <Eigen/Dense>
34 CPose2D::CPose2D(
const double x,
const double y,
const double _phi)
35 : m_phi(_phi), m_cossin_uptodate(false)
68 in >> x0 >> y0 >> phi0;
101 m_coords[0] =
static_cast<double>(in[
"x"]);
102 m_coords[1] =
static_cast<double>(in[
"y"]);
103 m_phi =
static_cast<double>(in[
"phi"]);
137 A.update_cached_cos_sin();
228 double lx,
double ly,
double lz,
double& gx,
double& gy,
double& gz)
const
237 const double gx,
const double gy,
double& lx,
double& ly)
const
340 : m_phi(o.phi), m_cossin_uptodate(false)
386 return (p1.
x() == p2.
x()) && (p1.
y() == p2.
y()) && (p1.
phi() == p2.
phi());
391 return (p1.
x() != p2.
x()) || (p1.
y() != p2.
y()) || (p1.
phi() != p2.
phi());
396 const double ccos = pose.
phi_cos();
397 const double ssin = pose.
phi_sin();
399 pose.
x() + u.
x * ccos - u.
y * ssin, pose.
y() + u.
x * ssin + u.
y * ccos);
429 4 * (1 - cos(p.
phi() -
phi())));
436 getHomogeneousMatrixVal<CMatrixDouble44>());
452 for (
int i = 0; i < 3; i++)
453 (*
this)[i] = std::numeric_limits<double>::quiet_NaN();
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
mrpt::math::TPose2D asTPose() const
mrpt::math::CVectorFixedDouble< 2 > m_coords
[x,y]
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
double phi() const
Get the phi angle of the 2D pose (in radians)
Virtual base class for "schematic archives" (JSON, XML,...)
void setToNaN() override
Set all data fields to quiet NaN.
double x() const
Common members of all points & poses classes.
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
CPose2D & operator+=(const CPose2D &b)
Make
double m_cosphi
Precomputed cos() & sin() of phi.
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
void inverse()
Convert this pose into its inverse, saving the result in itself.
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
mrpt::vision::TStereoCalibResults out
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CPose2D operator-(const CPose2D &b) const
Compute
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
#define THROW_EXCEPTION(msg)
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double distance2DFrobeniusTo(const CPose2D &p) const
Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.
CMatrixFixed< double, 4, 4 > CMatrixDouble44
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
void inverseComposeFrom(const CPose2D &A, const CPose2D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
Virtual base class for "archives": classes abstracting I/O streams.
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
void update_cached_cos_sin() const
A compile-time fixed-size numeric matrix container.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[0.02 1.04 -0....
void AddComponents(const CPose2D &p)
Scalar sum of components: This is diferent from poses composition, which is implemented as "+" operat...
std::string asString() const
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
constexpr double RAD2DEG(const double x)
Radians to degrees.
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
return_t square(const num_t x)
Inline function for the square of a number.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
constexpr double DEG2RAD(const double x)
Degrees to radians
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPose2D()
Default constructor (all coordinates to 0)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
void inverseComposePoint(const double gx, const double gy, double &lx, double &ly) const
Computes the 2D point L such as .
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
TPoint2D_< double > TPoint2D
Lightweight 2D point.
This base provides a set of functions for maths stuff.
MATRIX22 getRotationMatrix() const
CPose2D operator+(const CPose2D &D) const
The operator is the pose compounding operator.
MATRIX44 getInverseHomogeneousMatrixVal() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
size_type cols() const
Number of columns in the matrix.
A class used to store a 2D point.
A class used to store a 3D point.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void asVector(vector_t &v) const
Returns a 1x3 vector with [x y phi].
size_type rows() const
Number of rows in the matrix.
std::string std::string format(std::string_view fmt, ARGS &&... args)
void operator*=(const double s)
Scalar multiplication.
double m_phi
The orientation of the pose, in radians.
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020 | |