Go to the documentation of this file.
86 mutable double m_yaw, m_pitch, m_roll;
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))); }
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).
#define THROW_EXCEPTION(msg)
bool m_ypr_uptodate
Whether yaw/pitch/roll members are up-to-date since the last rotation matrix update.
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.
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...
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 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!...
#define ASSERTMSG_(f, __ERROR_MSG)
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...
#define ASSERT_EQUAL_(__A, __B)
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()
Default constructor, with all the coordinates set to zero.
std::ptrdiff_t difference_type
double pitch() const
Get the PITCH 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.
CPose3D(const math::CMatrixDouble &m)
Constructor from a 4x4 homogeneous matrix - the passed matrix can be actually of any size larger than...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
CPose3D(const CPoint3D &)
Constructor from a CPoint3D object.
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
A base class for representing a pose in 2D or 3D.
double z
X,Y,Z coordinates.
const type_value & getPoseMean() const
size_t size(const MATRIXLIKE &m, int dim)
CPose3D(const CPose3DQuat &)
Constructor from a CPose3DQuat.
bool isHorizontal(const double tolerance=0) const
Return true if the 6D pose represents a Z axis almost exactly vertical (upwards or downwards),...
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string asString() const
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...
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.
void getYawPitchRoll(double &yaw, double &pitch, double &roll) const
Returns the three angles (yaw, pitch, roll), in radians, from the rotation matrix.
#define ASSERT_BELOW_(__A, __B)
const double & const_reference
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 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::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 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...
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!...
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 fromString(const std::string &s)
Set the current object value from a string generated by 'asString' (eg: "[x y z yaw pitch roll]",...
double DEG2RAD(const double x)
Degrees to radians.
void changeCoordinatesReference(const CPose3D &p)
makes: this = p (+) this
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
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)
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 DEFINE_SERIALIZABLE_POST(class_name)
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 ln_jacob(mrpt::math::CMatrixFixedNumeric< double, 6, 12 > &J) const
Jacobian of the logarithm of the 3x4 matrix defined by this pose.
void normalizeAngles()
Rebuild the internal matrix & update the yaw/pitch/roll angles within the ]-PI,PI] range (Must be cal...
A class used to store a 2D pose.
const double & operator[](unsigned int i) const
void inverseComposePoint(const mrpt::math::TPoint2D &g, mrpt::math::TPoint2D &l, const double eps=1e-6) const
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).
double distanceEuclidean6D(const CPose3D &o) const
The euclidean distance between two poses taken as two 6-length vectors (angles in radians).
double roll() const
Get the ROLL angle (in radians)
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.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
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...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
void updateYawPitchRoll() const
Updates Yaw/pitch/roll members from the m_ROT
CPose3D(const CPose3DRotVec &p)
Constructor from a CPose3DRotVec.
#define DEFINE_SERIALIZABLE_PRE(class_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
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...
CPose3D(const math::CMatrixDouble44 &m)
Constructor from a 4x4 homogeneous matrix:
void rebuildRotationMatrix()
Rebuild the homog matrix from the angles.
A numeric matrix of compile-time fixed size.
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class.
void setToNaN() MRPT_OVERRIDE
Set all data fields to quiet NaN.
CPose3D(const CPose2D &)
Constructor from a CPose2D object.
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,...
void getAsVector(mrpt::math::CVectorDouble &v) const
Returns a 1x6 vector with [x y z yaw pitch roll].
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
double yaw() const
Get the YAW angle (in radians)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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...
static size_type max_size()
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...
const mrpt::math::CMatrixDouble33 & getRotationMatrix() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
#define ASSERT_ABOVEEQ_(__A, __B)
double value_type
The type of the elements.
void inverse()
Convert this pose into its inverse, saving the result in itself.
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...
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
mrpt::math::CArrayDouble< 6 > ln() const
This is an overloaded member function, provided for convenience. It differs from the above function o...
std::vector< T1 > & operator*=(std::vector< T1 > &a, const std::vector< T2 > &b)
a*=b (element-wise multiplication)
double RAD2DEG(const double x)
Radians to degrees.
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 getAsVector(mrpt::math::CArrayDouble< 6 > &v) const
This is an overloaded member function, provided for convenience. It differs from the above function o...
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.
static void resize(const size_t n)
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).
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.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
type_value & getPoseMean()
void addComponents(const CPose3D &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
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...
CPose3D(const mrpt::math::TPose3D &)
Constructor from lightweight object.
void asString(std::string &s) const
Returns a human-readable textual representation of the object (eg: "[x y z yaw pitch roll]",...
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods.
Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020 | | |