class mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Use this class instead of lightweight mrpt::math::TPose2D when pose/point composition is to be called multiple times with the same pose, since this class caches calls to expensive trigronometric functions.
For a complete description of Points/Poses, see mrpt::poses::CPoseOrPoint, or refer to this documentation page
- Read also: “A tutorial on SE(3) transformation parameterizations and
on-manifold optimization”, Jose-Luis Blanco. http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
See also:
#include <mrpt/poses/CPose2D.h> class CPose2D: public mrpt::poses::CPose, public mrpt::serialization::CSerializable, public mrpt::Stringifyable { public: // typedefs typedef double value_type; typedef double& reference; typedef double const_reference; typedef std::size_t size_type; typedef std::ptrdiff_t difference_type; typedef CPose2D type_value; // enums enum { is_3D_val = 0, }; enum { rotation_dimensions = 2, }; enum { is_PDF_val = 0, }; enum { static_size = 3, }; // fields mrpt::math::CVectorFixedDouble<2> m_coords; // construction CPose2D(); CPose2D(const double x, const double y, const double phi); CPose2D(const CPoint2D& p); CPose2D(const CPose3D& p); CPose2D(const mrpt::math::TPose2D& o); CPose2D(const CPoint3D& o); CPose2D(TConstructorFlags_Poses); // methods static constexpr size_type size(); static constexpr bool empty(); static constexpr size_type max_size(); static void resize(size_t n); mrpt::math::TPose2D asTPose() const; double phi() const; double& phi(); double phi_cos() const; double phi_sin() const; void phi(double angle); void phi_incr(const double Aphi); void asVector(vector_t& v) const; mrpt::math::TPoint2D translation() const; void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const; void getRotationMatrix(mrpt::math::CMatrixDouble22& R) const; void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const; template <class MATRIX22> MATRIX22 getRotationMatrix() const; CPose2D operator + (const CPose2D& D) const; void composeFrom(const CPose2D& A, const CPose2D& B); CPose3D operator + (const CPose3D& D) const; CPoint2D operator + (const CPoint2D& u) const; void composePoint(double lx, double ly, double& gx, double& gy) const; void composePoint(const mrpt::math::TPoint2D& l, mrpt::math::TPoint2D& g) const; mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D& l) const; void composePoint(const mrpt::math::TPoint3D& l, mrpt::math::TPoint3D& g) const; void composePoint(double lx, double ly, double lz, double& gx, double& gy, double& gz) const; void inverseComposePoint(const double gx, const double gy, double& lx, double& ly) const; void inverseComposePoint(const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l) const; mrpt::math::TPoint2D inverseComposePoint(const mrpt::math::TPoint2D& g) const; CPoint3D operator + (const CPoint3D& u) const; void inverseComposeFrom(const CPose2D& A, const CPose2D& B); void inverse(); CPose2D operator - (const CPose2D& b) const; CPose3D operator - (const CPose3D& b) const; void AddComponents(const CPose2D& p); void operator *= (const double s); CPose2D& operator += (const CPose2D& b); void normalizePhi(); CPose2D getOppositeScalar() const; virtual std::string asString() const; void fromString(const std::string& s); void fromStringRaw(const std::string& s); double operator [] (unsigned int i) const; double& operator [] (unsigned int i); void changeCoordinatesReference(const CPose2D& p); double distance2DFrobeniusTo(const CPose2D& p) const; const type_value& getPoseMean() const; type_value& getPoseMean(); virtual void setToNaN(); static CPose2D Identity(); static CPose2D FromString(const std::string& s); static constexpr bool is_3D(); static constexpr bool is_PDF(); };
Inherited Members
public: // methods double& x(); double& y(); void x(const double v); void y(const double v); void x_incr(const double v); void y_incr(const double v); const DERIVEDCLASS& derived() const; DERIVEDCLASS& derived();
Typedefs
typedef double value_type
The type of the elements.
typedef CPose2D type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Fields
mrpt::math::CVectorFixedDouble<2> m_coords
[x,y]
Construction
CPose2D()
Default constructor (all coordinates to 0)
CPose2D(const double x, const double y, const double phi)
Constructor from an initial value of the pose.
CPose2D(const CPoint2D& p)
Constructor from a CPoint2D object.
CPose2D(const CPose3D& p)
Aproximation!! Avoid its use, since information is lost.
CPose2D(const mrpt::math::TPose2D& o)
Constructor from lightweight object.
CPose2D(const CPoint3D& o)
Constructor from CPoint3D with information loss.
CPose2D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.
Methods
double phi() const
Get the phi angle of the 2D pose (in radians)
double& phi()
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
double phi_cos() const
Get a (cached) value of cos(phi), recomputing it only once when phi changes.
double phi_sin() const
Get a (cached) value of sin(phi), recomputing it only once when phi changes.
void phi(double angle)
Set the phi angle of the 2D pose (in radians)
void phi_incr(const double Aphi)
Increment the PHI angle (without checking the 2 PI range, call normalizePhi is needed)
void asVector(vector_t& v) const
Returns a 1x3 vector with [x y phi].
mrpt::math::TPoint2D translation() const
Returns the (x,y) translational part of the SE(2) transformation.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
See also:
void getRotationMatrix(mrpt::math::CMatrixDouble22& R) const
Returns the SE(2) 2x2 rotation matrix.
void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const
Returns the equivalent SE(3) 3x3 rotation matrix, with (2,2)=1.
CPose2D operator + (const CPose2D& D) const
The operator \(a = this \oplus D\) is the pose compounding operator.
void composeFrom(const CPose2D& A, const CPose2D& B)
Makes \(this = A \oplus B\).
A or B can be “this” without problems.
CPose3D operator + (const CPose3D& D) const
The operator \(a = this \oplus D\) is the pose compounding operator.
CPoint2D operator + (const CPoint2D& u) const
The operator \(u' = this \oplus u\) is the pose/point compounding operator.
void composePoint(double lx, double ly, double& gx, double& gy) const
An alternative, slightly more efficient way of doing \(G = P \oplus L\) with G and L being 2D points and P this 2D pose.
An alternative, slightly more efficient way of doing \(G = P \oplus L\) with G and L being 2D points and P this 2D pose.
void composePoint(const mrpt::math::TPoint2D& l, mrpt::math::TPoint2D& g) const
overload \(G = P \oplus L\) with G and L being 2D points and P this 2D pose
mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D& l) const
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
void composePoint(const mrpt::math::TPoint3D& l, mrpt::math::TPoint3D& g) const
overload \(G = P \oplus L\) with G and L being 3D points and P this 2D pose (the “z” coordinate remains unmodified)
void composePoint( double lx, double ly, double lz, double& gx, double& gy, double& gz ) const
overload (the “z” coordinate remains unmodified)
void inverseComposePoint( const double gx, const double gy, double& lx, double& ly ) const
Computes the 2D point L such as \(L = G \ominus this\).
See also:
void inverseComposePoint(const mrpt::math::TPoint2D& g, mrpt::math::TPoint2D& l) const
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
mrpt::math::TPoint2D inverseComposePoint(const mrpt::math::TPoint2D& g) const
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
CPoint3D operator + (const CPoint3D& u) const
The operator \(u' = this \oplus u\) is the pose/point compounding operator.
void inverseComposeFrom(const CPose2D& A, const CPose2D& B)
Makes \(this = A \ominus B\) this method is slightly more efficient than “this= A - B;” since it avoids the temporary object.
A or B can be “this” without problems.
See also:
void inverse()
Convert this pose into its inverse, saving the result in itself.
See also:
operator-
CPose2D operator - (const CPose2D& b) const
Compute \(RET = this \ominus b\)
CPose3D operator - (const CPose3D& b) const
The operator \(a \ominus b\) is the pose inverse compounding operator.
void AddComponents(const CPose2D& p)
Scalar sum of components: This is diferent from poses composition, which is implemented as “+” operators in “CPose” derived classes.
void operator *= (const double s)
Scalar multiplication.
CPose2D& operator += (const CPose2D& b)
Make \(this = this \oplus b\)
void normalizePhi()
Forces “phi” to be in the range [-pi,pi];.
CPose2D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individually.
virtual std::string asString() const
Returns a human-readable textual representation of the object (eg: “[x y yaw]”, yaw in degrees)
See also:
void fromString(const std::string& s)
Set the current object value from a string generated by ‘asString’ (eg: “[0.02 1.04 -0.8]” )
Parameters:
std::exception |
On invalid format |
See also:
void fromStringRaw(const std::string& s)
Same as fromString, but without requiring the square brackets in the string.
void changeCoordinatesReference(const CPose2D& p)
makes: this = p (+) this
double distance2DFrobeniusTo(const CPose2D& p) const
Returns the 2D distance from this pose/point to a 2D pose using the Frobenius distance.
virtual void setToNaN()
Set all data fields to quiet NaN.
static CPose2D Identity()
Returns the identity transformation.