75 inline T
r()
const {
return (*
this)[0];}
76 inline T
x()
const {
return (*
this)[1];}
77 inline T
y()
const {
return (*
this)[2];}
78 inline T
z()
const {
return (*
this)[3];}
79 inline void r(
const T
r) {(*this)[0]=
r;}
80 inline void x(
const T
x) {(*this)[1]=
x;}
81 inline void y(
const T
y) {(*this)[2]=
y;}
82 inline void z(
const T
z) {(*this)[3]=
z;}
95 template <
class ARRAYLIKE3>
103 const T angle = std::sqrt(x*x+y*y+z*z);
107 (*this)[1] =
static_cast<T
>(0.5)*
x;
108 (*this)[2] =
static_cast<T
>(0.5)*
y;
109 (*this)[3] =
static_cast<T
>(0.5)*
z;
113 const T s = (::sin(angle/2))/angle;
114 const T c = ::cos(angle/2);
131 template <
class ARRAYLIKE3>
132 inline void ln(ARRAYLIKE3 &out_ln)
const 134 if (out_ln.size()!=3) out_ln.resize(3);
138 template <
class ARRAYLIKE3>
139 inline ARRAYLIKE3
ln()
const 146 template <
class ARRAYLIKE3>
151 const T K = (xyz_norm<1e-7) ? 2 : 2*::acos(
r())/xyz_norm;
159 template <
class ARRAYLIKE3>
167 template <
class ARRAYLIKE3>
182 const T new_r = q1.
r()*q2.
r() - q1.
x()*q2.
x() - q1.
y()*q2.
y() - q1.
z()*q2.
z();
183 const T new_x = q1.
r()*q2.
x() + q2.
r()*q1.
x() + q1.
y()*q2.
z() - q2.
y()*q1.
z();
184 const T new_y = q1.
r()*q2.
y() + q2.
r()*q1.
y() + q1.
z()*q2.
x() - q2.
z()*q1.
x();
185 const T new_z = q1.
r()*q2.
z() + q2.
r()*q1.
z() + q1.
x()*q2.
y() - q2.
x()*q1.
y();
186 this->
r(new_r); this->
x(new_x); this->
y(new_y); this->
z(new_z);
192 void rotatePoint(
const double lx,
const double ly,
const double lz,
double &gx,
double &gy,
double &gz )
const 194 const double t2 =
r()*
x();
const double t3 =
r()*
y();
const double t4 =
r()*
z();
const double t5 =-
x()*
x();
const double t6 =
x()*
y();
195 const double t7 =
x()*
z();
const double t8 =-
y()*
y();
const double t9 =
y()*
z();
const double t10=-
z()*
z();
196 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
197 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
198 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
203 void inverseRotatePoint(
const double lx,
const double ly,
const double lz,
double &gx,
double &gy,
double &gz )
const 205 const double t2 =-
r()*
x();
const double t3 =-
r()*
y();
const double t4 =-
r()*
z();
const double t5 =-
x()*
x();
const double t6 =
x()*
y();
206 const double t7 =
x()*
z();
const double t8 =-
y()*
y();
const double t9 =
y()*
z();
const double t10=-
z()*
z();
207 gx = 2*((t8+ t10)*lx+(t6 - t4)*ly+(t3+t7)*lz)+lx;
208 gy = 2*((t4+ t6)*lx+(t5 +t10)*ly+(t9-t2)*lz)+ly;
209 gz = 2*((t7- t3)*lx+(t2 + t9)*ly+(t5+t8)*lz)+lz;
213 inline double normSqr()
const {
222 const T qq = 1.0/std::sqrt(
normSqr() );
223 for (
unsigned int i=0;i<4;i++)
230 template <
class MATRIXLIKE>
233 const T n = 1.0/std::pow(
normSqr(),T(1.5));
235 J.get_unsafe(0,0)=
x()*
x()+
y()*
y()+
z()*
z();
236 J.get_unsafe(0,1)=-
r()*
x();
237 J.get_unsafe(0,2)=-
r()*
y();
238 J.get_unsafe(0,3)=-
r()*
z();
240 J.get_unsafe(1,0)=-
x()*
r();
241 J.get_unsafe(1,1)=
r()*
r()+
y()*
y()+
z()*
z();
242 J.get_unsafe(1,2)=-
x()*
y();
243 J.get_unsafe(1,3)=-
x()*
z();
245 J.get_unsafe(2,0)=-
y()*
r();
246 J.get_unsafe(2,1)=-
y()*
x();
247 J.get_unsafe(2,2)=
r()*
r()+
x()*
x()+
z()*
z();
248 J.get_unsafe(2,3)=-
y()*
z();
250 J.get_unsafe(3,0)=-
z()*
r();
251 J.get_unsafe(3,1)=-
z()*
x();
252 J.get_unsafe(3,2)=-
z()*
y();
253 J.get_unsafe(3,3)=
r()*
r()+
x()*
x()+
y()*
y();
260 template <
class MATRIXLIKE>
264 J.get_unsafe(0,0)=
r(); J.get_unsafe(0,1)=-
x(); J.get_unsafe(0,2)=-
y(); J.get_unsafe(0,3)=-
z();
265 J.get_unsafe(1,0)=
x(); J.get_unsafe(1,1)=
r(); J.get_unsafe(1,2)=-
z(); J.get_unsafe(1,3)=
y();
266 J.get_unsafe(2,0)=
y(); J.get_unsafe(2,1)=
z(); J.get_unsafe(2,2)=
r(); J.get_unsafe(2,3)=-
x();
267 J.get_unsafe(3,0)=
z(); J.get_unsafe(3,1)=-
y(); J.get_unsafe(3,2)=
x(); J.get_unsafe(3,3)=
r();
279 template <
class MATRIXLIKE>
287 template <
class MATRIXLIKE>
290 M.get_unsafe(0,0)=
r()*
r()+
x()*
x()-
y()*
y()-
z()*
z(); M.get_unsafe(0,1)=2*(
x()*
y() -
r()*
z()); M.get_unsafe(0,2)=2*(
z()*
x()+
r()*
y());
291 M.get_unsafe(1,0)=2*(
x()*
y()+
r()*
z()); M.get_unsafe(1,1)=
r()*
r()-
x()*
x()+
y()*
y()-
z()*
z(); M.get_unsafe(1,2)=2*(
y()*
z()-
r()*
x());
292 M.get_unsafe(2,0)=2*(
z()*
x()-
r()*
y()); M.get_unsafe(2,1)=2*(
y()*
z()+
r()*
x()); M.get_unsafe(2,2)=
r()*
r()-
x()*
x()-
y()*
y()+
z()*
z();
319 rpy_and_jacobian(roll,pitch,yaw,static_cast<mrpt::math::CMatrixDouble*>(NULL));
327 template <
class MATRIXLIKE>
328 void rpy_and_jacobian(T &
roll, T &
pitch, T &yaw, MATRIXLIKE *out_dr_dq = NULL,
bool resize_out_dr_dq_to3x4 =
true )
const 333 if (out_dr_dq && resize_out_dr_dq_to3x4)
334 out_dr_dq->setSize(3,4);
335 const T discr =
r()*
y()-
x()*
z();
336 if (fabs(discr)>0.49999)
339 yaw =-2*atan2(
x(),
r());
343 out_dr_dq->get_unsafe(0,0) = +2/
x();
344 out_dr_dq->get_unsafe(0,2) = -2*
r()/(
x()*
x());
347 else if (discr<-0.49999)
350 yaw = 2*atan2(
x(),
r());
354 out_dr_dq->get_unsafe(0,0) = -2/
x();
355 out_dr_dq->get_unsafe(0,2) = +2*
r()/(
x()*
x());
360 yaw = ::atan2( 2*(
r()*
z()+
x()*
y()), 1-2*(
y()*
y()+
z()*
z()) );
361 pitch = ::asin ( 2*discr );
362 roll = ::atan2( 2*(
r()*
x()+
y()*
z()), 1-2*(
x()*
x()+
y()*
y()) );
365 const double val1=(2*
x()*
x() + 2*
y()*
y() - 1);
366 const double val12=
square(val1);
367 const double val2=(2*
r()*
x() + 2*
y()*
z());
368 const double val22=
square(val2);
369 const double xy2 = 2*
x()*
y();
370 const double rz2 = 2*
r()*
z();
371 const double ry2 = 2*
r()*
y();
372 const double val3 = (2*
y()*
y() + 2*
z()*
z() - 1);
373 const double val4 = ((
square(rz2 + xy2)/
square(val3) + 1)*val3);
374 const double val5 = (4*(rz2 + xy2))/
square(val3);
375 const double val6 = 1.0/(
square(rz2 + xy2)/
square(val3) + 1);
376 const double val7 = 2.0/ sqrt(1 -
square(ry2 - 2*
x()*
z()));
377 const double val8 = (val22/val12 + 1);
378 const double val9 = -2.0/val8;
380 out_dr_dq->get_unsafe(0,0) = -2*
z()/val4;
381 out_dr_dq->get_unsafe(0,1) = -2*
y()/val4;
382 out_dr_dq->get_unsafe(0,2) = -(2*
x()/val3 -
y()*val5)*val6 ;
383 out_dr_dq->get_unsafe(0,3) = -(2*
r()/val3 -
z()*val5)*val6;
385 out_dr_dq->get_unsafe(1,0) =
y()*val7 ;
386 out_dr_dq->get_unsafe(1,1) = -
z()*val7 ;
387 out_dr_dq->get_unsafe(1,2) =
r()*val7 ;
388 out_dr_dq->get_unsafe(1,3) = -
x()*val7 ;
390 out_dr_dq->get_unsafe(2,0) = val9*
x()/val1 ;
391 out_dr_dq->get_unsafe(2,1) = val9*(
r()/val1 - (2*
x()*val2)/val12) ;
392 out_dr_dq->get_unsafe(2,2) = val9*(
z()/val1 - (2*
y()*val2)/val12) ;
393 out_dr_dq->get_unsafe(2,3) = val9*
y()/val1 ;
void inverseRotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion...
static CQuaternion< T > exp(const ARRAYLIKE3 &v)
Exponential map from the SO(3) Lie Algebra to unit quaternions.
void rotationMatrix(MATRIXLIKE &M) const
Calculate the 3x3 rotation matrix associated to this quaternion: .
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
T square(const T x)
Inline function for the square of a number.
TConstructorFlags_Quaternions
T y() const
Return y coordinate of the quaternion.
#define THROW_EXCEPTION(msg)
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...
CQuaternion< double > CQuaternionDouble
A quaternion of data type "double".
double normSqr() const
Return the squared norm of the quaternion.
T r() const
Return r coordinate of the quaternion.
CQuaternion operator*(const T &factor)
void crossProduct(const CQuaternion &q1, const CQuaternion &q2)
Calculate the "cross" product (or "composed rotation") of two quaternion: this = q1 x q2 After the op...
void rotationJacobian(MATRIXLIKE &J) const
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix ...
void rotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void ln_noresize(ARRAYLIKE3 &out_ln) const
Like ln() but does not try to resize the output vector.
CArrayNumeric< T, 4 > Base
T square(const T x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
T x() const
Return x coordinate of the quaternion.
void normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
CQuaternion(const T r, const T x, const T y, const T z)
Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz...
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=NULL, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
T z() const
Return z coordinate of the quaternion.
#define ASSERTMSG_(f, __ERROR_MSG)
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
CQuaternion conj() const
Return the conjugate quaternion.
CQuaternion()
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.
CQuaternion(TConstructorFlags_Quaternions)
Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quat...
CQuaternion< float > CQuaternionFloat
A quaternion of data type "float".
void fromRodriguesVector(const ARRAYLIKE3 &v)
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If ...