9 #ifndef CPOSE3DROTVEC_H 10 #define CPOSE3DROTVEC_H 73 const double vx,
const double vy,
const double vz,
const double x,
74 const double y,
const double z)
142 out_HM.set_unsafe(0, 3,
m_coords[0]);
143 out_HM.set_unsafe(1, 3,
m_coords[1]);
144 out_HM.set_unsafe(2, 3,
m_coords[2]);
145 out_HM.set_unsafe(3, 0, 0);
146 out_HM.set_unsafe(3, 1, 0);
147 out_HM.set_unsafe(3, 2, 0);
148 out_HM.set_unsafe(3, 3, 1);
185 this->m_rotvec[0] = aux[0];
186 this->m_rotvec[1] = aux[1];
187 this->m_rotvec[2] = aux[2];
189 this->m_coords[0] = m.get_unsafe(0, 3);
190 this->m_coords[1] = m.get_unsafe(1, 3);
191 this->m_coords[2] = m.get_unsafe(2, 3);
195 const double x,
const double y,
const double z,
const double yaw = 0,
196 const double pitch = 0,
const double roll = 0);
203 double& out_pitch)
const;
213 double lx,
double ly,
double lz,
double& gx,
double& gy,
double& gz,
229 local_point.
x, local_point.
y, local_point.
z, global_point.
x,
230 global_point.
y, global_point.
z);
241 const double gx,
const double gy,
const double gz,
double& lx,
242 double& ly,
double& lz,
255 out_jacobian_drvtC_drvtA =
nullptr,
257 out_jacobian_drvtC_drvtB =
nullptr);
344 const double x0,
const double y0,
const double z0,
const double vx,
345 const double vy,
const double vz)
360 template <
class ARRAYORVECTOR>
378 template <
class ARRAYORVECTOR>
391 template <
class ARRAYORVECTOR>
415 throw std::runtime_error(
416 "CPose3DRotVec::operator[]: Index of bounds.");
436 throw std::runtime_error(
437 "CPose3DRotVec::operator[]: Index of bounds.");
466 if (!m.fromMatlabStringFormat(
s))
468 ASSERTMSG_(m.rows() == 1 && m.cols() == 6,
"Expected vector length=6");
469 for (
int i = 0; i < 3; i++)
m_coords[i] = m.get_unsafe(0, i);
470 for (
int i = 0; i < 3; i++)
m_rotvec[i] = m.get_unsafe(0, 3 + i);
528 static constexpr
bool empty() {
return false; }
533 throw std::logic_error(
format(
534 "Try to change the size of CPose3DRotVec to %u.",
535 static_cast<unsigned>(
n)));
541 std::ostream&
operator<<(std::ostream& o,
const CPose3DRotVec&
p);
545 CPose3DRotVec
operator-(
const CPose3DRotVec&
p);
547 bool operator==(
const CPose3DRotVec& p1,
const CPose3DRotVec& p2);
548 bool operator!=(
const CPose3DRotVec& p1,
const CPose3DRotVec& p2);
CPose3DRotVec(const double vx, const double vy, const double vz, const double x, const double y, const double z)
Constructor with initilization of the pose.
static CPose3DRotVec exp(const mrpt::math::CArrayDouble< 6 > &vect)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3DRotVec (static method)...
double x() const
Common members of all points & poses classes.
static constexpr bool empty()
CPose3DRotVec operator+(const CPose3DRotVec &b) const
The operator is the pose compounding operator.
void changeCoordinatesReference(const CPose3DRotVec &p)
makes: this = p (+) this
const double & operator[](unsigned int i) const
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 asString(std::string &s) const
Returns a human-readable textual representation of the object: "[x y z rx ry rz]".
const type_value & getPoseMean() const
#define THROW_EXCEPTION(msg)
void inverse()
Convert this pose into its inverse, saving the result in itself.
void setFromValues(const double x0, const double y0, const double z0, const double vx, const double vy, const double vz)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
CPose3DRotVec getInverse() const
Compute the inverse of this pose and return the result.
CPose3DRotVec operator-(const CPose3DRotVec &b) const
Compute .
static constexpr bool is_PDF()
GLdouble GLdouble GLdouble GLdouble q
void setToNaN() override
Set all data fields to quiet NaN.
void addComponents(const CPose3DRotVec &p)
Scalar sum of all 6 components: This is diferent from poses composition, which is implemented as "+" ...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
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 is_3D()
void setFromTransformationMatrix(const mrpt::math::CMatrixDouble44 &m)
double & operator[](unsigned int i)
const double & const_reference
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=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr) const
Computes the 3D point L such as .
const mrpt::math::CMatrixDouble33 getRotationMatrix() const
static constexpr size_type size()
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 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.
void getAs6Vector(ARRAYORVECTOR &vec6) const
Gets pose as an array with these 6 elements: [x y z vx vy vz] where v{xyz} is the rotation vector and...
void setFrom6Vector(const ARRAYORVECTOR &vec6)
Set pose from an array with these 6 elements: [x y z vx vy vz] where v{xyz} is the rotation vector an...
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...
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::math::CMatrixFixedNumeric< double, 3, 3 > *out_jacobian_df_dpoint=nullptr, mrpt::math::CMatrixFixedNumeric< double, 3, 6 > *out_jacobian_df_dpose=nullptr) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
void operator*=(const double s)
Scalar multiplication of x,y,z,vx,vy,vz.
A base class for representing a pose in 2D or 3D.
mrpt::math::CArrayDouble< 3 > rotVecFromRotMat(const math::CMatrixDouble44 &m)
Create a vector with 3 components according to the input transformation matrix (only the rotation wil...
double x
X,Y,Z coordinates.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
GLsizei const GLchar ** string
A class used to store a 2D point.
A class used to store a 3D point.
static constexpr size_type max_size()
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
static void resize(const size_t n)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
type_value & getPoseMean()
CPose3DRotVec(TConstructorFlags_Poses constructor_dummy_param)
Fast constructor that leaves all the data uninitialized - call with UNINITIALIZED_POSE as argument...
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix part of this pose, generating the corresponding vector ...
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
CPose3DRotVec(const mrpt::math::CArrayDouble< 6 > &v)
Constructor with initilization of the pose from a vector [w1 w2 w3 x y z].
CPose3DRotVec & operator+=(const CPose3DRotVec &b)
Make (b can be "this" without problems)
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
void toQuatXYZ(CPose3DQuat &q) const
Convert this RVT into a quaternion + XYZ.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
void inverseComposeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B)
Makes this method is slightly more efficient than "this= A - B;" since it avoids the temporary objec...
void composeFrom(const CPose3DRotVec &A, const CPose3DRotVec &B, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtA=nullptr, mrpt::math::CMatrixFixedNumeric< double, 6, 6 > *out_jacobian_drvtC_drvtB=nullptr)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
CPose3DRotVec(const CPose3DRotVec &o)
Copy constructor.
CPose3DRotVec & operator=(const CPose3DRotVec &o)
Copy operator.
CPose3DRotVec(const double *vec6)
Constructor from an array with these 6 elements: [w1 w2 w3 x y z] where r{ij} are the entries of the ...
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 getAsVector(ARRAYORVECTOR &v) const
Like getAs6Vector() but for dynamic size vectors (required by base class CPoseOrPoint) ...
double value_type
The type of the elements.
CPose3DRotVec()
Default constructor, with all the coordinates set to zero.
std::ptrdiff_t difference_type
std::string asString() const
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].