36 m_coords[0] = m(0, 3);
37 m_coords[1] = m(1, 3);
38 m_coords[2] = m(2, 3);
40 m_rotvec = rotVecFromRotMat(m);
50 m_rotvec = rotVecFromRotMat(
R);
62 const double a = sqrt(
q.x() *
q.x() +
q.y() *
q.y() +
q.z() *
q.z());
63 const double TH = 0.001;
64 const double k =
a < TH ? 2 : 2 * acos(
q.r()) / sqrt(1 -
q.r() *
q.r());
65 m_rotvec[0] = k *
q.x();
66 m_rotvec[1] = k *
q.y();
67 m_rotvec[2] = k *
q.z();
80 out << m_coords[0] << m_coords[1] << m_coords[2] << m_rotvec[0]
81 << m_rotvec[1] << m_rotvec[2];
95 in >> m_coords[0] >> m_coords[1] >> m_coords[2] >> m_rotvec[0] >>
96 m_rotvec[1] >> m_rotvec[2];
105 const double x,
const double y,
const double z,
const double yaw,
109 this->m_coords[0] = aux.
m_coords[0];
110 this->m_coords[1] = aux.
m_coords[1];
111 this->m_coords[2] = aux.
m_coords[2];
128 const std::streamsize old_pre = o.precision();
129 const std::ios_base::fmtflags old_flags = o.flags();
130 o <<
"(x,y,z,vx,vy,vz)=(" << std::fixed << std::setprecision(4)
131 <<
p.m_coords[0] <<
"," <<
p.m_coords[1] <<
"," <<
p.m_coords[2] <<
"," 132 <<
p.m_rotvec[0] <<
"," <<
p.m_rotvec[1] <<
"," <<
p.m_rotvec[2] <<
")";
134 o.precision(old_pre);
150 const TPoint3D& point,
double& out_range,
double& out_yaw,
151 double& out_pitch)
const 155 this->inverseComposePoint(
159 out_range =
local.norm();
169 out_pitch = -asin(
local.z / out_range);
178 double lx,
double ly,
double lz,
double& gx,
double& gy,
double& gz,
182 const double angle = this->m_rotvec.norm();
183 const double K1 = sin(angle) / angle;
184 const double K2 = (1 - cos(angle)) / (angle * angle);
186 const double tx = this->m_coords[0];
187 const double ty = this->m_coords[1];
188 const double tz = this->m_coords[2];
190 const double w1 = this->m_rotvec[0];
191 const double w2 = this->m_rotvec[1];
192 const double w3 = this->m_rotvec[2];
194 const double w1_2 =
w1 *
w1;
195 const double w2_2 =
w2 *
w2;
196 const double w3_2 = w3 * w3;
198 gx = lx * (1 - K2 * (w2_2 + w3_2)) + ly * (K2 *
w1 *
w2 - K1 * w3) +
199 lz * (K1 *
w2 + K2 *
w1 * w3) + tx;
200 gy = lx * (K1 * w3 + K2 *
w1 *
w2) + ly * (1 - K2 * (w1_2 + w3_2)) +
201 lz * (K2 *
w2 * w3 - K1 *
w1) +
ty;
202 gz = lx * (K2 *
w1 * w3 - K1 *
w2) + ly * (K1 *
w1 + K2 *
w2 * w3) +
203 lz * (1 - K2 * (w1_2 + w2_2)) +
tz;
205 if (out_jacobian_df_dpoint || out_jacobian_df_dpose)
218 b.getInverseHomogeneousMatrix(B_INV);
240 b.m_coords[0],
b.m_coords[1],
b.m_coords[2], outPoint.
m_coords[0],
254 b.m_coords[0],
b.m_coords[1], 0, outPoint.
m_coords[0],
262 q.m_coords[0] = this->m_coords[0];
263 q.m_coords[1] = this->m_coords[1];
264 q.m_coords[2] = this->m_coords[2];
266 const double a = sqrt(
267 this->m_rotvec[0] * this->m_rotvec[0] +
268 this->m_rotvec[1] * this->m_rotvec[1] +
269 this->m_rotvec[2] * this->m_rotvec[2]);
273 q.m_quat.x(0.5 * this->m_rotvec[0]);
274 q.m_quat.y(0.5 * this->m_rotvec[1]);
275 q.m_quat.z(0.5 * this->m_rotvec[2]);
281 q.m_quat.fromRodriguesVector(this->m_rotvec);
318 if (
a1 < 0.01 &&
a2 < 0.01)
326 if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
328 if (out_jacobian_drvtC_drvtA)
330 out_jacobian_drvtC_drvtA->setIdentity(6, 6);
331 out_jacobian_drvtC_drvtA->insertMatrix(
335 if (out_jacobian_drvtC_drvtB)
337 out_jacobian_drvtC_drvtB->setIdentity(6, 6);
338 out_jacobian_drvtC_drvtB->insertMatrix(
340 (*out_jacobian_drvtC_drvtB)(3, 3) = (*out_jacobian_drvtC_drvtB)(
341 4, 4) = (*out_jacobian_drvtC_drvtB)(5, 5) = 1;
353 this->m_coords[0] =
coords[0];
354 this->m_coords[1] =
coords[1];
355 this->m_coords[2] =
coords[2];
359 else if (A_is_small) this->m_rotvec = B.
m_rotvec;
360 else if (B_is_small) this->m_rotvec = A.
m_rotvec;
363 const double a1_inv = 1/
a1;
364 const double a2_inv = 1/
a2;
366 const double sin_a1_2 = sin(0.5*
a1);
367 const double cos_a1_2 = cos(0.5*
a1);
368 const double sin_a2_2 = sin(0.5*
a2);
369 const double cos_a2_2 = cos(0.5*
a2);
371 const double KA = sin_a1_2*sin_a2_2;
372 const double KB = sin_a1_2*cos_a2_2;
373 const double KC = cos_a1_2*sin_a2_2;
374 const double KD = cos_a1_2*cos_a2_2;
376 const double r11 = a1_inv*A.
m_rotvec[0];
377 const double r12 = a1_inv*A.
m_rotvec[1];
378 const double r13 = a1_inv*A.
m_rotvec[2];
380 const double r21 = a2_inv*B.
m_rotvec[0];
381 const double r22 = a2_inv*B.
m_rotvec[1];
382 const double r23 = a2_inv*B.
m_rotvec[2];
384 const double q3[] = {
385 KD - KA*(r11*r21+r12*r22+r13*r23),
386 KC*r21 + KB*r11 + KA*(r22*r13-r23*r12),
387 KC*r22 + KB*r12 + KA*(r23*r11-r21*r13),
388 KC*r23 + KB*r13 + KA*(r21*r12-r22*r11)
391 const double param = 2*acos(q3[0])/sqrt(1-q3[0]*q3[0]);
392 this->m_rotvec[0] =
param*q3[1];
393 this->m_rotvec[1] =
param*q3[2];
394 this->m_rotvec[2] =
param*q3[3];
410 if (out_jacobian_drvtC_drvtA || out_jacobian_drvtC_drvtB)
415 const double& qCr = qC.
m_quat[0];
416 const double& qCx = qC.
m_quat[1];
417 const double& qCy = qC.
m_quat[2];
418 const double& qCz = qC.
m_quat[3];
420 const double& r1 = this->m_rotvec[0];
421 const double& r2 = this->m_rotvec[1];
422 const double& r3 = this->m_rotvec[2];
424 const double C = 1 / (1 - qCr * qCr);
425 const double D = acos(qCr) / sqrt(1 - qCr * qCr);
427 alignas(16)
const double aux_valsH[] = {
428 2 * C * qCx * (D * qCr - 1), 2 * D, 0, 0,
429 2 * C * qCy * (D * qCr - 1), 0, 2 * D, 0,
430 2 * C * qCz * (D * qCr - 1), 0, 0, 2 * D};
434 const double alpha = sqrt(r1 * r1 + r2 * r2 + r3 * r3);
438 alignas(16)
const double aux_valsG[] = {
439 -r1 * alpha2 * sin(
alpha / 2),
440 -r2 * alpha2 * sin(
alpha / 2),
441 -r3 * alpha2 * sin(
alpha / 2),
442 2 * alpha2 * sin(
alpha / 2) + r1 * r1 * KA,
446 2 * alpha2 * sin(
alpha / 2) + r2 * r2 * KA,
450 2 * alpha2 * sin(
alpha / 2) + r3 * r3 * KA};
454 if (out_jacobian_drvtC_drvtB)
458 const double& qAr = qA.
m_quat[0];
459 const double& qAx = qA.
m_quat[1];
460 const double& qAy = qA.
m_quat[2];
461 const double& qAz = qA.
m_quat[3];
463 alignas(16)
const double aux_valsQA[] = {
464 qAr, -qAx, -qAy, -qAz, qAx, qAr, qAz, -qAy,
465 qAy, -qAz, qAr, qAx, qAz, qAy, -qAx, qAr};
469 jac_rot_B.multiply_ABC(H, QA, G);
471 out_jacobian_drvtC_drvtB->fill(0);
472 out_jacobian_drvtC_drvtB->insertMatrix(0, 0, jac_rot_B);
473 out_jacobian_drvtC_drvtB->insertMatrix(3, 3, RA);
475 if (out_jacobian_drvtC_drvtA)
479 const double& qBr = qB.
m_quat[0];
480 const double& qBx = qB.
m_quat[1];
481 const double& qBy = qB.
m_quat[2];
482 const double& qBz = qB.
m_quat[3];
484 alignas(16)
const double aux_valsQB[] = {
485 qBr, -qBx, -qBy, -qBz, qBx, qBr, -qBz, qBy,
486 qBy, qBz, qBr, -qBx, qBz, -qBy, qBx, qBr};
490 jac_rot_A.multiply_ABC(H, QB, G);
493 out_jacobian_drvtC_drvtA->fill(0);
494 out_jacobian_drvtC_drvtA->insertMatrix(0, 0, jac_rot_A);
495 out_jacobian_drvtC_drvtB->insertMatrix(3, 3, id3);
504 this->getInverseHomogeneousMatrix(B_INV);
505 this->setFromTransformationMatrix(B_INV);
513 this->getInverseHomogeneousMatrix(B_INV);
540 HM_C.multiply_AB(HM_B_inv, HM_A);
542 this->m_rotvec = this->rotVecFromRotMat(HM_C);
544 this->m_coords[0] = HM_C(0, 3);
545 this->m_coords[1] = HM_C(1, 3);
546 this->m_coords[2] = HM_C(2, 3);
553 const double gx,
const double gy,
const double gz,
double& lx,
double& ly,
582 result.head<3>() = m_coords;
583 result.tail<3>() = m_rotvec;
588 for (
int i = 0; i < 3; i++)
590 m_coords[i] = std::numeric_limits<double>::quiet_NaN();
591 m_rotvec[i] = std::numeric_limits<double>::quiet_NaN();
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.
GLclampf GLclampf GLclampf alpha
CPose3DRotVec operator+(const CPose3DRotVec &b) const
The operator is the pose compounding operator.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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).
void inverse()
Convert this pose into its inverse, saving the result in itself.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
void setFromXYZAndAngles(const double x, const double y, const double z, const double yaw=0, const double pitch=0, const double roll=0)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CPose3DRotVec getInverse() const
Compute the inverse of this pose and return the result.
mrpt::math::CArrayDouble< 3 > m_coords
[x,y,z]
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
mrpt::math::CArrayDouble< 3 > ln_rotation() const
Take the logarithm of the 3x3 rotation matrix, generating the corresponding vector in the Lie Algebra...
void setToNaN() override
Set all data fields to quiet NaN.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void setFromTransformationMatrix(const mrpt::math::CMatrixDouble44 &m)
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 .
void setRotationMatrix(const mrpt::math::CMatrixDouble33 &ROT)
Sets the 3x3 rotation matrix.
const mrpt::math::CMatrixDouble33 getRotationMatrix() const
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 skew_symmetric3_neg(const VECTOR &v, MATRIX &M)
Computes the negative version of a 3x3 skew symmetric matrix from a 3-vector or 3-array: ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z] access directly or with x(), y(), z() setter/getter methods...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool operator!=(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 inverse homogeneous transformation matrix for this point or pose...
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...
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.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
A class used to store a 2D point.
A class used to store a 3D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble GLdouble w2
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).
mrpt::math::CArrayDouble< 3 > m_rotvec
The rotation vector [vx,vy,vz].
void toQuatXYZ(CPose3DQuat &q) const
Convert this RVT into a quaternion + XYZ.
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...
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
mrpt::math::CQuaternionDouble m_quat
The quaternion.
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 ...
GLubyte GLubyte GLubyte a
CPose3DRotVec()
Default constructor, with all the coordinates set to zero.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].
GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint GLdouble w1
mrpt::math::CArrayDouble< 3 > m_coords
The translation vector [x,y,z].