86 mutable double m_yaw, m_pitch, m_roll;
89 void rebuildRotationMatrix();
92 inline void updateYawPitchRoll()
const {
if (!m_ypr_uptodate) { m_ypr_uptodate=
true; getYawPitchRoll( m_yaw, m_pitch, m_roll ); } }
102 CPose3D(
const double x,
const double y,
const double z,
const double yaw=0,
const double pitch=0,
const double roll=0);
111 template <
class MATRIX33,
class VECTOR3>
115 for (
int r=0;r<3;r++)
116 for (
int c=0;c<3;c++)
117 m_ROT(r,c)=rot.get_unsafe(r,c);
118 for (
int r=0;r<3;r++) m_coords[r]=xyz[r];
126 explicit CPose3D(
const CPose2D &);
153 setFrom12Vector(vec12);
168 out_HM.insertMatrix(0,0,m_ROT);
169 for (
int i=0;i<3;i++) out_HM(i,3)=m_coords[i];
170 out_HM(3,0)=out_HM(3,1)=out_HM(3,2)=0.; out_HM(3,3)=1.;
204 void sphericalCoordinates(
208 double &out_pitch )
const;
216 void composePoint(
double lx,
double ly,
double lz,
double &gx,
double &gy,
double &gz,
220 bool use_small_rot_approx =
false)
const;
226 composePoint(local_point.
x,local_point.
y,local_point.
z, global_point.
x,global_point.
y,global_point.
z );
231 composePoint(local_point.
x,local_point.
y,local_point.
z, global_point.
x,global_point.
y,dummy_z );
235 inline void composePoint(
double lx,
double ly,
double lz,
float &gx,
float &gy,
float &gz )
const {
237 composePoint(lx,ly,lz,ggx,ggy,ggz);
238 gx =
static_cast<float>(ggx); gy =
static_cast<float>(ggy); gz =
static_cast<float>(ggz);
247 void inverseComposePoint(
const double gx,
const double gy,
const double gz,
double &lx,
double &ly,
double &lz,
254 inverseComposePoint(g.
x,g.
y,g.
z, l.
x,l.
y,l.
z);
260 inverseComposePoint(g.
x,g.
y,0, l.
x,l.
y,lz);
267 void composeFrom(
const CPose3D& A,
const CPose3D& B );
272 composeFrom(*
this,b);
280 void inverseComposeFrom(
const CPose3D& A,
const CPose3D& B );
305 void addComponents(
const CPose3D &p);
310 void normalizeAngles();
323 const double pitch=0,
324 const double roll=0);
329 template <
typename VECTORLIKE>
332 const size_t index_offset = 0)
338 m_ypr_uptodate=
false;
339 m_coords[0] = v[index_offset+0];
340 m_coords[1] = v[index_offset+1];
341 m_coords[2] = v[index_offset+2];
352 setFromValues(
x(),y(),
z(),yaw_,pitch_,roll_);
359 template <
class ARRAYORVECTOR>
362 m_ROT.set_unsafe(0,0, vec12[0]); m_ROT.set_unsafe(0,1, vec12[3]); m_ROT.set_unsafe(0,2, vec12[6]);
363 m_ROT.set_unsafe(1,0, vec12[1]); m_ROT.set_unsafe(1,1, vec12[4]); m_ROT.set_unsafe(1,2, vec12[7]);
364 m_ROT.set_unsafe(2,0, vec12[2]); m_ROT.set_unsafe(2,1, vec12[5]); m_ROT.set_unsafe(2,2, vec12[8]);
365 m_ypr_uptodate =
false;
366 m_coords[0] = vec12[ 9];
367 m_coords[1] = vec12[10];
368 m_coords[2] = vec12[11];
375 template <
class ARRAYORVECTOR>
378 vec12[0] = m_ROT.get_unsafe(0,0); vec12[3] = m_ROT.get_unsafe(0,1); vec12[6] = m_ROT.get_unsafe(0,2);
379 vec12[1] = m_ROT.get_unsafe(1,0); vec12[4] = m_ROT.get_unsafe(1,1); vec12[7] = m_ROT.get_unsafe(1,2);
380 vec12[2] = m_ROT.get_unsafe(2,0); vec12[5] = m_ROT.get_unsafe(2,1); vec12[8] = m_ROT.get_unsafe(2,2);
381 vec12[ 9] = m_coords[0];
382 vec12[10] = m_coords[1];
383 vec12[11] = m_coords[2];
389 void getYawPitchRoll(
double &yaw,
double &
pitch,
double &
roll )
const;
391 inline double yaw()
const { updateYawPitchRoll();
return m_yaw; }
392 inline double pitch()
const { updateYawPitchRoll();
return m_pitch; }
393 inline double roll()
const { updateYawPitchRoll();
return m_roll; }
405 void getAsQuaternion(
412 updateYawPitchRoll();
415 case 0:
return m_coords[0];
416 case 1:
return m_coords[1];
417 case 2:
return m_coords[2];
419 case 4:
return m_pitch;
420 case 5:
return m_roll;
422 throw std::runtime_error(
"CPose3D::operator[]: Index of bounds.");
442 if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION(
"Malformed expression in ::fromString");
444 this->setFromValues(m.get_unsafe(0,0),m.get_unsafe(0,1),m.get_unsafe(0,2),
DEG2RAD(m.get_unsafe(0,3)),
DEG2RAD(m.get_unsafe(0,4)),
DEG2RAD(m.get_unsafe(0,5)));
448 bool isHorizontal(
const double tolerance=0)
const;
451 double distanceEuclidean6D(
const CPose3D &o )
const;
494 static void jacob_dexpeD_de(
const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
498 static void jacob_dAexpeD_de(
const CPose3D &A,
const CPose3D &D, Eigen::Matrix<double,12,6> & jacob);
505 enum { is_3D_val = 1 };
506 static inline bool is_3D() {
return is_3D_val!=0; }
507 enum { rotation_dimensions = 3 };
508 enum { is_PDF_val = 0 };
509 static inline bool is_PDF() {
return is_PDF_val!=0; }
526 static inline bool empty() {
return false; }
528 static inline void resize(
const size_t n) {
if (n!=
static_size)
throw std::logic_error(
format(
"Try to change the size of CPose3D to %u.",static_cast<unsigned>(n))); }
#define ASSERT_EQUAL_(__A, __B)
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...
double DEG2RAD(const double x)
Degrees to radians.
const type_value & getPoseMean() const
CPose2D BASE_IMPEXP operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
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 MRPT_OVERRIDE
C++11 "override" for virtuals:
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It's not safe to set t...
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
#define THROW_EXCEPTION(msg)
#define ASSERT_BELOW_(__A, __B)
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...
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
double pitch() const
Get the PITCH angle (in radians)
double yaw() const
Get the YAW angle (in radians)
CPose3D(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
double z
X,Y,Z coordinates.
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
std::ptrdiff_t difference_type
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CArrayDouble< 3 > &xyz)
const double & const_reference
double value_type
The type of the elements.
CPose3D(const mrpt::math::CArrayDouble< 12 > &vec12)
Constructor from an array with these 12 elements: [r11 r21 r31 r12 r22 r32 r13 r23 r33 tx ty tz] wher...
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]"...
A numeric matrix of compile-time fixed size.
type_value & getPoseMean()
double RAD2DEG(const double x)
Radians to degrees.
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
static void resize(const size_t n)
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
A base class for representing a pose in 2D or 3D.
CPose3D type_value
Used to emulate CPosePDF types, for example, in mrpt::graphs::CNetworkOfPoses.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
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 inverseComposePoint(const mrpt::math::TPoint3D &g, mrpt::math::TPoint3D &l) const
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
A class used to store a 2D point.
A class used to store a 3D point.
double roll() const
Get the ROLL angle (in radians)
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 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 asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]"...
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
overload for 2D points
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define ASSERT_ABOVEEQ_(__A, __B)
size_t size(const MATRIXLIKE &m, const int dim)
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
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...
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
#define DEFINE_SERIALIZABLE_POST(class_name)
mrpt::math::CArrayDouble< 6 > ln() const
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
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 ...
#define ASSERTMSG_(f, __ERROR_MSG)
static size_type max_size()
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
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!) ...
std::string asString() const
const double & operator[](unsigned int i) const
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].