Go to the documentation of this file.
135 const double x,
const double y,
const double z,
const double yaw = 0,
136 const double pitch = 0,
const double roll = 0);
152 template <
class MATRIX33,
class VECTOR3>
153 inline CPose3D(
const MATRIX33& rot,
const VECTOR3& xyz)
159 for (
int r = 0; r < 3; r++)
160 for (
int c = 0; c < 3; c++)
m_ROT(r, c) = rot(r, c);
161 for (
int r = 0; r < 3; r++)
m_coords[r] = xyz[r];
267 double& out_pitch)
const;
281 double lx,
double ly,
double lz,
double& gx,
double& gy,
double& gz,
288 bool use_small_rot_approx =
false)
const;
300 local_point.
x, local_point.
y, local_point.
z, global_point.
x,
301 global_point.
y, global_point.
z);
320 local_point.
x, local_point.
y, local_point.
z, global_point.
x,
321 global_point.
y, dummy_z);
327 double lx,
double ly,
double lz,
float& gx,
float& gy,
float& gz)
const
329 double ggx, ggy, ggz;
354 const double gx,
const double gy,
const double gz,
double& lx,
355 double& ly,
double& lz,
382 const double eps = 1e-6)
const
459 const double x0,
const double y0,
const double z0,
const double yaw = 0,
460 const double pitch = 0,
const double roll = 0);
467 template <
typename VECTORLIKE>
468 inline void setFromXYZQ(
const VECTORLIKE& v,
const size_t index_offset = 0)
473 v[index_offset + 3], v[index_offset + 4], v[index_offset + 5],
474 v[index_offset + 6]);
487 const double yaw_,
const double pitch_,
const double roll_)
498 template <
class ARRAYORVECTOR>
501 m_ROT(0, 0) = vec12[0];
502 m_ROT(0, 1) = vec12[3];
503 m_ROT(0, 2) = vec12[6];
504 m_ROT(1, 0) = vec12[1];
505 m_ROT(1, 1) = vec12[4];
506 m_ROT(1, 2) = vec12[7];
507 m_ROT(2, 0) = vec12[2];
508 m_ROT(2, 1) = vec12[5];
509 m_ROT(2, 2) = vec12[8];
522 template <
class ARRAYORVECTOR>
525 vec12[0] =
m_ROT(0, 0);
526 vec12[3] =
m_ROT(0, 1);
527 vec12[6] =
m_ROT(0, 2);
528 vec12[1] =
m_ROT(1, 0);
529 vec12[4] =
m_ROT(1, 1);
530 vec12[7] =
m_ROT(1, 2);
531 vec12[2] =
m_ROT(2, 0);
532 vec12[5] =
m_ROT(2, 1);
533 vec12[8] =
m_ROT(2, 2);
605 throw std::runtime_error(
606 "CPose3D::operator[]: Index of bounds.");
698 static constexpr
bool empty() {
return false; }
700 static inline void resize(
const size_t n)
703 throw std::logic_error(
format(
704 "Try to change the size of CPose3D to %u.",
705 static_cast<unsigned>(n)));
711 std::ostream&
operator<<(std::ostream& o,
const CPose3D& p);
717 bool operator==(
const CPose3D& p1,
const CPose3D& p2);
718 bool operator!=(
const CPose3D& p1,
const CPose3D& p2);
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
void setFrom12Vector(const ARRAYORVECTOR &vec12)
Set pose from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] where r...
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]",...
void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint2D &global_point) const
This version of the method assumes that the resulting point has no Z component (use with caution!...
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
CPose3D(const mrpt::math::CVectorFixedDouble< 12 > &vec12)
Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] wher...
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
double pitch() const
Get the PITCH angle (in radians)
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
void sphericalCoordinates(const mrpt::math::TPoint3D &point, double &out_range, double &out_yaw, double &out_pitch) const
Computes the spherical coordinates of a 3D point as seen from the 6D pose specified by this object.
mrpt::math::TPoint3D inverseComposePoint(const mrpt::math::TPoint3D &g) const
double x() const
Common members of all points & poses classes.
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
A base class for representing a pose in 2D or 3D.
const type_value & getPoseMean() const
void setToNaN() override
Set all data fields to quiet NaN.
static constexpr bool is_3D()
static constexpr size_type max_size()
mrpt::math::TVector3D inverseRotateVector(const mrpt::math::TVector3D &global) const
Inverse of rotateVector(), i.e.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
static constexpr size_type size()
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
CVectorFixed< double, N > CVectorFixedDouble
Specialization of CVectorFixed for double numbers.
std::string asString() const
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
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...
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It's not safe to set t...
CPose3D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.
void setYawPitchRoll(const double yaw_, const double pitch_, const double roll_)
Set the 3 angles of the 3D pose (in radians) - This method recomputes the internal rotation coordinat...
#define ASSERT_BELOW_(__A, __B)
CPose3D operator+(const CPose3D &b) const
The operator is the pose compounding operator.
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
void getAs12Vector(ARRAYORVECTOR &vec12) const
Get the pose representation as an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 ...
A compile-time fixed-size numeric matrix container.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
static constexpr bool empty()
void composePoint(double lx, double ly, double lz, float &gx, float &gy, float &gz) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
CPose3D operator-(const CPose3D &b) const
Compute
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.
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
overload for 2D points
bool operator!=(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::math::TPose3D asTPose() const
double m_yaw
These variables are updated every time that the object rotation matrix is modified (construction,...
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards),...
double roll() const
Get the ROLL angle (in radians)
DECLARE_MEXPLUS_FROM(mrpt::img::TCamera) namespace std
static CPose3D Identity()
Returns the identity transformation.
mrpt::math::TVector3D rotateVector(const mrpt::math::TVector3D &local) const
Rotates a vector (i.e.
The virtual base class which provides a unified interface for all persistent objects in MRPT.
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CVectorFixedDouble< 3 > &xyz)
This is an overloaded member function, provided for convenience. It differs from the above function o...
float d2f(const double d)
shortcut for static_cast<float>(double)
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
CPose3D & operator+=(const CPose3D &b)
Make (b can be "this" without problems)
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt) const
Computes the 3D point L such as .
void operator*=(const double s)
Scalar multiplication of x,y,z,yaw,pitch & roll (angles will be wrapped to the ]-pi,...
std::ptrdiff_t difference_type
void inverseComposeFrom(const CPose3D &A, const CPose3D &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
CPose3D getOppositeScalar() const
Return the opposite of the current pose instance by taking the negative of all its components individ...
#define ASSERT_ABOVEEQ_(__A, __B)
void inverse()
Convert this pose into its inverse, saving the result in itself.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
CPose3D(const MATRIX33 &rot, const VECTOR3 &xyz)
Constructor from a 3x3 rotation matrix and a the translation given as a 3-vector, a 3-array,...
static CPose3D FromString(const std::string &s)
CPose3D()
Default constructor, with all the coordinates set to zero.
double operator[](unsigned int i) const
double yaw() const
Get the YAW angle (in radians)
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
void setFromXYZQ(const VECTORLIKE &v, const size_t index_offset=0)
Set the pose from a 3D position (meters) and a quaternion, stored as [x y z qr qx qy qz] in a 7-eleme...
#define DEFINE_SCHEMA_SERIALIZABLE()
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
mrpt::math::CVectorFixedDouble< DIM > vector_t
Fixed-size vector of the correct size to hold all the coordinates of the point/pose.
mrpt::math::CVectorFixedDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
void composePoint(const mrpt::math::TPoint3D &local_point, mrpt::math::TPoint3D &global_point) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::optional_ref< mrpt::math::CMatrixDouble43 > out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
static void resize(const size_t n)
A class used to store a 2D point.
A class used to store a 3D point.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
double value_type
The type of the elements.
static constexpr bool is_PDF()
void fromStringRaw(const std::string &s)
Same as fromString, but without requiring the square brackets in the string.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
void inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
void asVector(vector_t &v) const
Returns a 6x1 vector with [x y z yaw pitch roll]'.
type_value & getPoseMean()
std::string std::string format(std::string_view fmt, ARGS &&... args)
mrpt::math::TPoint3D composePoint(const mrpt::math::TPoint3D &l) const
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]",...
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |