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];
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.;
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 = ggx; gy = ggy; gz = 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);
272 composeFrom(*
this,b);
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];
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; }
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)));
510 enum { is_3D_val = 1 };
511 static inline bool is_3D() {
return is_3D_val!=0; }
512 enum { rotation_dimensions = 3 };
513 enum { is_PDF_val = 0 };
514 static inline bool is_PDF() {
return is_PDF_val!=0; }
531 static inline bool empty() {
return false; }
533 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 DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class.
#define DEFINE_SERIALIZABLE_POST(class_name)
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
A numeric matrix of compile-time fixed size.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
void rotationMatrixNoResize(MATRIXLIKE &M) const
Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quate...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
A class used to store a 2D point.
A class used to store a 3D point.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
CPose3D(const CPose3DQuat &)
Constructor from a CPose3DQuat.
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...
static void ln_rot_jacob(const mrpt::math::CMatrixDouble33 &R, mrpt::math::CMatrixFixedNumeric< double, 3, 9 > &M)
Static function to compute the Jacobian of the SO(3) Logarithm function, evaluated at a given 3x3 rot...
void getAsQuaternion(mrpt::math::CQuaternionDouble &q, mrpt::math::CMatrixFixedNumeric< double, 4, 3 > *out_dq_dr=NULL) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
CPose3D(const CPose2D &)
Constructor from a CPose2D object.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
CPose3D(const math::CMatrixDouble44 &m)
Constructor from a 4x4 homogeneous matrix:
CPose3D(const mrpt::math::TPose3D &)
Constructor from lightweight object.
void getAsVector(mrpt::math::CArrayDouble< 6 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
void ln(mrpt::math::CArrayDouble< 6 > &out_ln) const
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the...
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
std::string asString() const
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...
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
mrpt::math::CArrayDouble< 6 > ln() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
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...
std::ptrdiff_t difference_type
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
double pitch() const
Get the PITCH angle (in radians)
CPose3D(const math::CMatrixDouble &m)
Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than...
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
double roll() const
Get the ROLL angle (in radians)
CPose3D(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
Constructor with initilization of the pose; (remember that angles are always given in radians!...
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards),...
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...
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,...
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!...
static void exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, CPose3D &out_pose, bool pseudo_exponential=false)
This is an overloaded member function, provided for convenience. It differs from the above function o...
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]",...
static CPose3D exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, bool pseudo_exponential=false)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
const double & const_reference
static mrpt::math::CMatrixDouble33 exp_rotation(const mrpt::math::CArrayNumeric< double, 3 > &vect)
Exponentiate a vector in the Lie algebra to generate a new SO(3) (a 3x3 rotation matrix).
static void jacob_dAexpeD_de(const CPose3D &A, const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.
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(TConstructorFlags_Poses)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument.
static void resize(const size_t n)
const type_value & getPoseMean() const
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
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(const mrpt::math::CQuaternionDouble &q, const double x, const double y, const double z)
Constructor from a quaternion (which only represents the 3D rotation part) and a 3D displacement.
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...
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
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 ln_jacob(mrpt::math::CMatrixFixedNumeric< double, 6, 12 > &J) const
Jacobian of the logarithm of the 3x4 matrix defined by this pose.
CPose3D(const mrpt::math::CMatrixDouble33 &rot, const mrpt::math::CArrayDouble< 3 > &xyz)
This is an overloaded member function, provided for convenience. It differs from the above function o...
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL, 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...
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...
double value_type
The type of the elements.
void fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]",...
double yaw() const
Get the YAW angle (in radians)
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.
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x6 vector with [x y z yaw pitch roll].
const double & operator[](unsigned int i) const
CPose3D(const CPose3DRotVec &p)
Constructor from a CPose3DRotVec.
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
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...
type_value & getPoseMean()
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 ...
void inverseComposePoint(const double gx, const double gy, const double gz, double &lx, double &ly, double &lz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=NULL, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dse3=NULL) const
Computes the 3D point L such as .
CPose3D(const CPoint3D &)
Constructor from a CPoint3D object.
static void jacob_dexpeD_de(const CPose3D &D, Eigen::Matrix< double, 12, 6 > &jacob)
The Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.
CPose3D()
Default constructor, with all the coordinates set to zero.
static size_type max_size()
void inverse()
Convert this pose into its inverse, saving the result in itself.
mrpt::math::CMatrixDouble33 m_ROT
The 3x3 rotation matrix, access with getRotationMatrix(), setRotationMatrix() (It's not safe to set t...
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
A 3D pose, with a 3D translation and a rotation in 3D parameterized in rotation-vector form (equivale...
A base class for representing a pose in 2D or 3D.
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
#define ASSERT_EQUAL_(__A, __B)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
#define ASSERT_BELOW_(__A, __B)
#define THROW_EXCEPTION(msg)
#define ASSERTMSG_(f, __ERROR_MSG)
#define ASSERT_ABOVEEQ_(__A, __B)
size_t size(const MATRIXLIKE &m, int dim)
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...
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.
double DEG2RAD(const double x)
Degrees to radians.
double RAD2DEG(const double x)
Radians to degrees.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double z
X,Y,Z coordinates.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).